EKF: 14 state INS with z-axis bias and mag heading mode #1506
Conversation
|
Testing and code review extremely welcome! I feel quite good about this code. |
| for (j = i; j < NUMX; j++) | ||
| D[i][j] = P[i][j]; | ||
|
|
||
| // Brute force calculation of the elements of P |
There was a problem hiding this comment.
Where does this come from? Would be good to have some explanation.
There was a problem hiding this comment.
That's pretty old code from the 13 state INS, just generic matrix multiplication unrolled.
|
I read through the code but didn't verify the math. Do you have the matlab scripts used to generate the update steps? It would help for double checking things. I will test the next time I go flying. |
|
@BrainFPV I tried to write the python code so it was a bit more understandable and "mathy", I can dig out some of the matlab code that generated some of the pseudocode too but even that is fairly cryptic and the output requires a fair bit of massaging |
|
One issue I'm seeing with this is that BaroAltitude is showing up in the SystemStats.ObjectManagerQueueID so somewhere that queue overflows (when not receiving GPS data). |
|
@elafargue thsi should be good to test if you want to |
peabody124
force-pushed
the
ins14
branch
2 times, most recently
from
b52b582 to
004ff77
Compare
This reverts commit caec9ad.
For some reason D[15][15] was being replaced with D[14][15] in the automatically computed code which prevented the z-axis accel bias calculation working properly. This was fixed manually (and tediously).
These variables are not referenced anywhere in the covariance prediction, which is where G is used.
This is for symmetry with the way the gyro biases are treated.
This makes it converge faster and still seems quite stable.
This is a testing mode, primarily, so it is nice to use the same settings as in the real mode.
The velocity was updated towards zero for indoor mode and the ublox vertical velocity in outdoor mode. Further testing has indicated that in indoor mode this overly damps the velocity estimate. For outdoor mode, it will likely have issues because of the latency in the GPS. The baro is a pretty accurate sensor and sufficient to fuse with the accels to get a good estimate of the vertical velocity.
The last row of plots was not useful.
Sync the X-axis
This simulates the bias changing after the system converges to check how long it takes to stabilize.
Also update the simulation to keep the cirlce centered at the orgin rather than drifting away.
Conflicts: python/ins/ins14.py
This catches any problems with the attitude coupling into the yaw, which is one of the things this code is designed to fix.
The calculation to remove the influence of attitude from the mag was transposed which applied in the wrong way and magnified problems.
Was only passing in the first two axes which prevents backing out the transformation.
Under some rare conditions, the INS biases can get very large and destabilize the filter. Because these are meant to track the errors in sensors, we can place reasonable priors on what values are valid and saturate at these limits..
This is more consistent with the other objects. More critically, it invalidates any previous settings without mag horizontal plane compensation. The mag is now used in real units so the variance increased by about two orders of magnitude. This resulted in NANs from the EKF.
This module takes almost 20KB of flash and is rarely used. This disables it by default, but leaves it commented to make it easy to reenable for custom compiles.
|
While I hope for more work in this area.. INS14 is clearly better, the patch reads sane, and we have good flight test on it. |
EKF: 14 state INS with z-axis bias and mag heading mode As flight-tested during the Spring 2015 Tau Labs meetup!
When engaging altitude hold using the INS it is common to see a
small glitch in the altitude. This arises because of untracked bias
in the accelerometer. This new INS fixes this issue by tracking
the accelerometer bias.
In addition it changes how the mag is dealt with so it only influences
yaw and cannot affect roll and pitch. This creates a better attitude
estimate in practice.
Finally it adds unit tests for the INS with regards to stability and
convergence rates.
Much of this work is written up here: http://buildandcrash.blogspot.com/2015/01/ekf-enhancements-and-unit-tests.html