beagleboard / beagleboard/librobotcontrol
kalman.c should have separate prediction and correction steps
- Dominant language
- C
- Stars
- 212
- Forks
- 167
- PR merge metrics
- No merged PRs in 30d
Description
**Is your feature request related to a problem? Please describe.**
`rc_kalman_update_lin()` and `rc_kalman_update_ekf()` both current do the prediction and correction steps of the Kalman filter at the same time and with no way to separate the two. This presents a problem when the input sensor (used for prediction step) is sampled more frequently than the "measurement sensor" (used for the correction step).
Take the altitude estimation on the BeagleBoneBlue for example. The MPU is sampled at 200Hz giving us accelerometer readings for the prediction step at 200Hz. However, the barometer (used for the correction step) is only sampled at 20Hz. With the currently-available functions, the user must run the Kalman filter at 200Hz with "stale", un-changing barometer measurements for several iterations of the filter.
This problem only gets worse when you add in a GPS which refreshes values at like 4Hz.
**Describe the solution you'd like**
I think both the linear KF and the non-linear EKF should have separate functions for prediction and correction. I propose new functions (two each for linear and nonlinear filtering).
`rc_kalman_predict_lin() `should only do the following prediction steps
- x_pre[k|k-1] = F * x[k-1|k-1] + G * u[k-1]
- P[k|k-1] = F * P[k-1|k-1 ] *F^T + Q
`rc_kalman_correct_lin()` should only do the following correction steps
- h[k] = H * x_pre[k]
- S = H * P * H^T + R
- L = P * (H^T) * (S^-1)
- x_est[k|k] = x[k|k-1] + L * (y[k]-h[k])
- P[k|k] = (I - L * H) * P[k|k-1]
`rc_kalman_update_lin()` should then just call `rc_kalman_predict_lin()` and` rc_kalman_correct_lin()` sequentially to maintain its functionality.
There should also be `rc_kalman_predict_ekf()` and `rc_kalman_correct_ekf()` which only take in the needed Jacobians to do prediction and correction respectively. `rc_kalman_update_ekf()` should simply call these two new EKF functions sequentially.
**Describe alternatives you've considered**
It's possible to not add in any new functions and instead use` rc_kalman_update_lin()` and `rc_kalman_update_ekf()`, but add in two booleans. One boolean will select for the prediction step and the other will select for the correction step. If both booleans are true, the current functionality of `rc_kalman_update_lin()` will be retained. If both are false, the function should just exit immediately. I'm not sure if adding in new inputs to the pre-existing functions will present backwards-compatibility issues.
It's also possible to use the existing functions and input zero-matrices for the steps that aren't wanted. For example, if I only want the correction step, I can input F, G, and Q as zero matrices. However, this is really clunky, wastes computation cycles (both in re-assigning the matrices and in doing math just to get 0 as the resultant vector), and won't work if I want prediction without correction. To eliminate the correction step alone using this method, I'd have to make H and R zero matrices which will cause a singularity when inverting S = H * P * H^T + R.
**Additional context**
This library has been really useful for my research. Thanks for making this open-source and continuing to update it :)
Contributor guide
No contributing guide indexed for this repository
Assessment
This issue has not been assessed yet.