fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 03:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use matrix lib inside getState

Browse Source
This commit is contained in:
kamilritz 2020-06-27 10:14:12 +02:00 committed by Mathieu Bresciani
parent 16a00eae0b
commit 97225fcb74
1 changed files with 8 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
EKF/ekf_helper.cpp
View File

@ -840,37 +840,14 @@ void Ekf::get_gps_check_status(uint16_t *val)
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
{
matrix::Vector<float, 24> state;
for (int i = 0; i < 4; i++) {
state(i) = _state.quat_nominal(i);
}
for (int i = 0; i < 3; i++) {
state(i + 4) = _state.vel(i);
}
for (int i = 0; i < 3; i++) {
state(i + 7) = _state.pos(i);
}
for (int i = 0; i < 3; i++) {
state(i + 10) = _state.delta_ang_bias(i);
}
for (int i = 0; i < 3; i++) {
state(i + 13) = _state.delta_vel_bias(i);
}
for (int i = 0; i < 3; i++) {
state(i + 16) = _state.mag_I(i);
}
for (int i = 0; i < 3; i++) {
state(i + 19) = _state.mag_B(i);
}
for (int i = 0; i < 2; i++) {
state(i + 22) = _state.wind_vel(i);
}
state.slice<4, 1>(0, 0) = _state.quat_nominal;
state.slice<3, 1>(4, 0) = _state.vel;
state.slice<3, 1>(7, 0) = _state.pos;
state.slice<3, 1>(10, 0) = _state.delta_ang_bias;
state.slice<3, 1>(13, 0) = _state.delta_vel_bias;
state.slice<3, 1>(16, 0) = _state.mag_I;
state.slice<3, 1>(19, 0) = _state.mag_B;
state.slice<2, 1>(22, 0) = _state.wind_vel;
return state;
}