dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Initialise variances to zero for un-used states 

Setting these variances to zero makes it less likely that these states will be modified by fusion processing.
This commit is contained in:
Paul Riseborough 2016-02-16 10:42:18 +11:00
parent d9bf4e9870
commit 69b8982043
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
EKF/covariance.cpp
View File

@ -81,19 +81,22 @@ void Ekf::initialiseCovariance()
// accel z bias
P[15][15] = 0.0001f;
// variances for optional states
// these state variances are set to zero until the states are required, then they must be initialised
// earth magnetic field
P[16][16] = 0.0001f;
P[17][17] = 0.0001f;
P[18][18] = 0.0001f;
P[16][16] = 0.0f;
P[17][17] = 0.0f;
P[18][18] = 0.0f;
// body magnetic field
P[19][19] = 0.0001f;
P[20][20] = 0.0001f;
P[21][21] = 0.0001f;
P[19][19] = 0.0f;
P[20][20] = 0.0f;
P[21][21] = 0.0f;
// wind
P[22][22] = 0.01f;
P[23][23] = 0.01f;
P[22][22] = 0.0f;
P[23][23] = 0.0f;
}