fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make Kfusion a Vector24f

Browse Source
This commit is contained in:
kamilritz
2020-07-02 09:02:24 +02:00
committed by Mathieu Bresciani
parent 356b2f6ffe
commit 16a00eae0b
11 changed files with 619 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+9 -34
View File
@@ -1126,42 +1126,17 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
*status = soln_status.value;
}
// fuse measurement
void Ekf::fuse(float *K, float innovation)
void Ekf::fuse(const Vector24f& K, float innovation)
{
for (unsigned i = 0; i < 4; i++) {
_state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
}
_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
_state.quat_nominal.normalize();
for (unsigned i = 0; i < 3; i++) {
_state.vel(i) = _state.vel(i) - K[i + 4] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.pos(i) = _state.pos(i) - K[i + 7] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.delta_ang_bias(i) = _state.delta_ang_bias(i) - K[i + 10] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.delta_vel_bias(i) = _state.delta_vel_bias(i) - K[i + 13] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
}
for (unsigned i = 0; i < 2; i++) {
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
}
_state.vel -= K.slice<3, 1>(4, 0) * innovation;
_state.pos -= K.slice<3, 1>(7, 0) * innovation;
_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;
_state.delta_vel_bias -= K.slice<3, 1>(13, 0) * innovation;
_state.mag_I -= K.slice<3, 1>(16, 0) * innovation;
_state.mag_B -= K.slice<3, 1>(19, 0) * innovation;
_state.wind_vel -= K.slice<2, 1>(22, 0) * innovation;
}
void Ekf::uncorrelateQuatFromOtherStates()