aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: use observation noise to set initial magnetic field variances

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-24 11:08:33 +10:00 committed by Andrew Tridgell
parent 830751c0ae
commit ac329ec31c
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1350,11 +1350,11 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
zeroRows(P,16,21); zeroRows(P,16,21);
zeroCols(P,16,21); zeroCols(P,16,21);
// set initial earth magnetic field variances // set initial earth magnetic field variances
P[16][16] = sq(0.05f); P[16][16] = sq(frontend->_magNoise);
P[17][17] = P[16][16]; P[17][17] = P[16][16];
P[18][18] = P[16][16]; P[18][18] = P[16][16];
// set initial body magnetic field variances // set initial body magnetic field variances
P[19][19] = sq(0.05f); P[19][19] = sq(frontend->_magNoise);
P[20][20] = P[19][19]; P[20][20] = P[19][19];
P[21][21] = P[19][19]; P[21][21] = P[19][19];