I’d like to learn more about the EKF and how it estimates the states. By looking at the position and velocity estimates I can see that the position changes don’t usually match with the velocity estimates, as shown in the image below. Why is this? Shouldn’t be obvious for the EKF that the vertical velocity must be around zero when the altitude is not changing? instead, it has a positive bias. Same logic with the horizontal movement, while there is a positive change in position it also estimating a negative velocity.
Sorry if this is a dumb question, but this is throwing me off a bit.
Thanks!
The EKF combines the IMU’s acceleration output with the barometer (vertically) or GPS (horizontally) but sometimes the results don’t match especially in cases of high vibration. If the vehicle is suffering from high vibration then the IMU values can clip leading to a loss of information. There can also be “aliasing” (high rate vibrations that coincidentally match the IMU’s sampling speed) which can lead to inaccurate acceleration values.
The EKF has the accelerations and the positions but the velocities are in the middle so they might not match either the integrated accelerations nor the change in position.
You may want to take a look how the equations are generated. There is an script for calculating the Jacobians and the covariance matrices here: https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_NavEKF3/derivation . Specifically, you may want to check the method generate_code() in main.py. Here you will find the definition of the state space model, and since it is an Extended Kalman filter the linearization.
I am not sure how familiar you are with the theory of Kalman filtering, so if you need to sharpen your knowledge I recommend you the book Optimal state estimation by Dan Simon or this course by Gregory Plett.
Still having much to learn about how the Kalman filter of Ardupilot works, in my short experience I have realized that is imperative to read the source code at certain point. The classes NavEKF3 and NavEKF3_core are where almost everything happens. You will find that it is not only an Extended Kalman filter what is inside there. For example, you will find algorithms for compensating the delay between sensors that have different sample rates, and a logic flow that makes the algorithm behaves differently in situations such as if the vehicle is in ground or flying.
