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

Make observation jacobian a Vector24f when possible

Browse Source
This commit is contained in:
kamilritz
2020-07-16 18:25:06 +02:00
committed by Mathieu Bresciani
parent b5e1397c0f
commit 5c4a3d4576
4 changed files with 104 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+11 -11
View File
@@ -71,12 +71,12 @@ void Ekf::fuseAirspeed()
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
// Observation Jacobian
float H_TAS[24] = {};
H_TAS[4] = SH_TAS[2];
H_TAS[5] = SH_TAS[1];
H_TAS[6] = vd*SH_TAS[0];
H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1];
Vector24f H_TAS;
H_TAS(4) = SH_TAS[2];
H_TAS(5) = SH_TAS[1];
H_TAS(6) = vd*SH_TAS[0];
H_TAS(22) = -SH_TAS[2];
H_TAS(23) = -SH_TAS[1];
_airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
@@ -161,11 +161,11 @@ void Ekf::fuseAirspeed()
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion(row) * H_TAS[4];
KH[1] = Kfusion(row) * H_TAS[5];
KH[2] = Kfusion(row) * H_TAS[6];
KH[3] = Kfusion(row) * H_TAS[22];
KH[4] = Kfusion(row) * H_TAS[23];
KH[0] = Kfusion(row) * H_TAS(4);
KH[1] = Kfusion(row) * H_TAS(5);
KH[2] = Kfusion(row) * H_TAS(6);
KH[3] = Kfusion(row) * H_TAS(22);
KH[4] = Kfusion(row) * H_TAS(23);
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(4,column);