I’ve been going over the EKF2 implementation in PX4, and I must say it is done very well! I’m checking the code because we are busy working with the new uBlox M8P RTK chips, and I want to make sure that there aren’t any limitations we might run into down the line.
I have found however that the parameter EKF2_GPS_P_NOISE should be set lower (we are using 0.02 at the moment). Same goes for EKF2_GPS_V_NOISE (also used 0.02). The default is 0.5, which is too high given the cm level accuracy promised by the uBlox M8P.
Given that we have a very accurate GPS, I would like the estimator to trust the GPS measurement update more.
I was wondering if you could help me with the following:
Could you send me any mathematical papers you have describing the maths behind EKF2?
Should we also be looking at changing EKF2_TAU_POS & EKF2_TAU_VEL ? (this seems to be a complimentary filter time-constant that combines previous state and current state measurements in order to get rid of some of the delays, but I’m not 100% sure)
What other parameters would you suggest we should change in order to optimize our EKF2 for the uBlox RTK system?
I have looked at the InertialNav repository on GitHub, is this something we could use to optimize our EKF2 parameters?
Checkout the in src/lib/ecl/matlab/scripts/IntertialNavEkf/GenerateNavFilterEquations.m for the derivation of the extended kalman filter (EKF). The EKF is actually running on a delayed time horizon in order to enable fusing measurements at their matching time. Therefore, there is a complementary filter which produces the inertial solution at the non-delayed time horizon and using the EKF output
for correcting it’s state.
You would need to have a look at your log and evaluate if the complementary filter is tracking the ekf output to your satisfaction. That would depend on your system. I think we log the innovations of the complementary filter, I think EST4.e1 to e3.
You are right about the gps noise parameters for RTK. However, you should still have a look at your position and velocity innovations and innovation variances in the log and tune them according to your needs. In general I would recommend you to use the replay tool documented here http://dev.px4.io/ekf2_log_replay.html
One you have captured a single replay log on your physical system you can use the replay tool
to tune the ekf parameters. You will see the effect o parameter changes on the filter outputs.
Q) Could you send me any mathematical papers you have describing the maths behind EKF2?
Ans) The Khosrovian paper listed in the documentation was the original inspiration for the output observer, however the output observers current implementation has been modified substantially from that. For the state equations, covariance prediction, etc, see the derivation script that tumbili has listed .
Q) Should we also be looking at changing EKF2_TAU_POS & EKF2_TAU_VEL ? (this seems to be a complimentary filter time-constant that combines previous state and current state measurements in order to get rid of some of the delays, but I’m not 100% sure)
Ans) The role of the output filter is two-fold. Firstly to predict forward from the EKF’s fusion time horizon to current time and secondly to filter out observation noise. I would be looking to reduce the time constants if you have reasonably low noise aiding data (good GPS). The EST4.e1 EST4.e2 and EST4.e3 show the magnitude of the angular, velocity and position tracking error of the output predictor measured at the fusion time horizon. You can use the velocity (EST4.e2) and position (EST4.e3) errors to tune your time constants for the required compromise between tracking accuracy and observation noise.
Q) What other parameters would you suggest we should change in order to optimize our EKF2 for the uBlox RTK system?
Ans) One thing to try for better accuracy is to tune the value of EKF2_GPS_DELAY to match your receiver. The default is value is 200msec which was the value for the old UBlox 6 receiver hardware. If your receiver has lower delay, then you can alse reduce the size of the EKF time horizon delay by reducing https://github.com/PX4/ecl/blob/master/EKF/estimator_interface.h#L277 from the current value of 300msec (30 samples) to a value about 20msec larger than your GPS delay. That will tighten output predictor tracking significantly.
Finding the optimum value of EKF2_GPS_DELAY is best done using the EKF2 replay, adjusting the value of the parameter and picking the value that minimises the RMS GPS velocity innovations.
Q) I have looked at your InertialNav repository on GitHub, is this something we could use to optimize our EKF2 parameters?
Ans) I second the recommendation posted by tumbili for this one. Using the replay tool is a good option, but you will need a good SD card. This page has some information on different card brands: http://dev.px4.io/advanced-logging.html
Great, thanks Paul! This gives me much more insight into the EKF. I’ll strt by working with the log replay, then I’ll look at the exact delay of the uBlox M8P. Thanks again for the help!
So just a couple of follow-up questions to make sure I’m not misunderstanding your advice:
(my understanding) The goal is to try to minimize both EST4.e2 (velocity innovation) and EST4.e3 (position innovation) by adjusting EKF2_GPS_DELAY, EKF2_TAU_POS and EKF2_TAU_VEL.
(question) Is it required to fly the vehicle when logging the EKF replay data, or is it sufficient to have it powered on, motors on, on the ground?
(question) You mentioned that there is a compromise between tracking accuracy and observation noise. Is it enough just to look at minimizing EST4.e2 and EST4.e3 to achieve this compromise, or is there another parameter I should also be looking at?
Adjust the EKF2_GPS_DELAY using the replay to minimise the main filter NE GPS velocity innovations EST4.VxI and EST4.VyI.
Reduce EKF2_TAU_VEL to reduce the size of the output complementary filter/predictor tracking error EST4.e2 (velocity tracking error magnitude) as much as you can and still have acceptable levels of ‘stepping’ due to measurement fusion in the local position velocity outputs LPOS.VX. LPOS.VY and LPOS.VZ
Reduce EKF2_TAU_POS to reduce the size of the output complementary filter/predictor tracking error EST4.e3 (position tracking error magnitude) as much as you can and still have acceptable levels of ‘stepping’ due to measurement fusion in the local position position outputs LPOS.X. LPOS.Y and LPOS.Z
Also check ATT.Roll, ATT.Pitch and ATT.Yaw for rising noise when you are doing 2) and 3). They are normally OK, but some setups can have noisier angular state estimates.
Don’t forget to check the ESC demands in the logs when you first fly with the new parameters. If you have introduced too much noise, it will show up in the ESC demands.
Q1) You won’t get realistic data (vibration and other sensor errors) on the ground
Q2) Look at the LPOS X, VX, etc as outlines in 2) and 3)
I am also trying to tune EKF for RTK. I recorder some log for reply, but I am not sure how to use properly. Is there some kind of a GUI which plots figures, that I can compare, each time I change the replay_params.txt?
@Paul_Riseborough Do you know why the innovation gates are constrained at a maximum of 1.0f in code? For example here: gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
I am using the EKF2 in combination with ublox M8P, but I found that lots of good measurements are rejected. After a deep dive in the code I came to the conclusion that I need to increase the innovation gate sizes. Unless I am missing something, I think this hard-coded constraint could be removed completely.
@Paul_Riseborough Oops that’s stupid, I guess my brain was overloaded and I interpreted fmaxf as constraining it to maximum of 1. Thanks for the quick reply!
