kalman.c should have separate prediction and correction steps

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() andrc_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 userc_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 :)

Indeed the Kalman module is quite minimalist. I put together just enough for what I needed for my own research and haven't gone back to it. On my wish list was also to remove a lot of the duplicate code by having the linear functions just wrap the extended equivalent functions, passing in the static matrices as arguments. There is also a lot of optimization that can be done with regards to memory allocation. Right now all the intermediary steps have required memory allocated and freed each step.

Another note is right now rc_kalman_update_ekf doesn't do any prediction, the user has to pass in their predicted state x_pre. This was to allow for highly-nonlinear systems to be modeled by the user.

I like the idea of having separate functions for predictor and corrector more than passing bools as arguments, it's easier to follow code with descriptive function names and it wouldn't require changing existing APIs.

I've not tried running a KF with different sensor sample rates but it's definitely a common use case. Is there some literature you could reference to describe the best way to handle multiple sample rates (as opposed to a simple zero-order-hold). If you fork this repo and draft some of these ideas I can work with you to review and merge them back in,

Mr Strawson Design, Good to hear from you. I didn't see this in your distro. I want to use the DMP for sorta inertial nav unit on a ground vehicle. Part of sensor fusion for localization. I found your rc_altitude.c example which shows how to set up the KF, access the DMP + BMP, do a filter. A missing factoid that I need is the DMP sensor noise to use in the KF. From rc_altitude.c it looks like you used 1000000.0. As I try to decode the example, it looks like you associate this with the vertical acceleration. So do you have a feeling for the noise figures for velocity and position? I looked through your other examples and I know you spent a lot of time working on flying things. Have you put any knoodles into using the DMP for helping with ground vehicle localization? Thanks for any help you can provide.

Hi Clark! Hope you are doing well.

I picked that number via experiment and it's much larger than the actual covariance of the IMU data. I did that to force the filter to converge on the Barometer more quickly and get the behavior I was looking for.

The traditional kamlan filter is optimum for sensor data with purely gaussian noise which is never true in reality. TDK publish noise values for the IMU but these are under ideal conditions. The noise you will experience will be due to aliasing and frame vibrations. I'd suggest starting with 1s and work up and down in orders of magnitude.

For something with 2 inputs like the altitude estimator, it behaves very much like Bewley's explanation of the simple 1st order crossover filter. Adjusting the noise values in the kalman filter shift the crossover frequently up and down, favoring the barometer or accelerometer respectively.

On Tue, Jun 9, 2020, 09:08 clarkbriggs notifications@github.com wrote:

Mr Strawson Design, Good to hear from you. I didn't see this in your distro. I want to use the DMP for sorta inertial nav unit on a ground vehicle. Part of sensor fusion for localization. I found your rc_altitude.c example which shows how to set up the KF, access the DMP + BMP, do a filter. A missing factoid that I need is the DMP sensor noise to use in the KF. From rc_altitude.c it looks like you used 1000000.0. As I try to decode the example, it looks like you associate this with the vertical acceleration. So do you have a feeling for the noise figures for velocity and position? I looked through your other examples and I know you spent a lot of time working on flying things. Have you put any knoodles into using the DMP for helping with ground vehicle localization? Thanks for any help you can provide.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/beagleboard/librobotcontrol/issues/185#issuecomment-641404681, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABPUPICNTAJGGZKE53DOMZ3RVZM7RANCNFSM4NYQMSWA .

Heyo. I've spent some time over the last few weeks working with my implementation of separate prediction and correction functions. I've made these changes in a forked version of librobotcontrol. Here's a rundown of changes that I think are important to highlight.

There are now separate 'rc_kalman_predict_lin()' and 'rc_kalman_correct_lin()' functions -- Ideally, 'rc_kalman_update_lin()' should be made to call these two new functions in sequence, but I did not implement this in my forked version of the repo.
For the correction step, I changed the Kalman Gain matrix 'L' to be named 'K'. This follows better with how the Kalman gain matrix is usually represented mathematically and makes the code a bit easier to read (and compare to equations) in my opinion
The correction step in the new 'rc_kalman_correct_lin()' now checks to see if the matrix 'S' was successfully inverted. If 'S' is not invertible, the function returns with -1 and neither the state vector nor the covariance matrix are changed.
The integer 'step' is only incremented in the 'predict' function. -- I did this because when working with sensors that sample at different rates, you will sometimes get multiple correction sensors in one iteration and want to run the 'correction' function separately for each measurement (even though they arrived at the same iteration).

I haven't changed this in my forked version of the code, but in my work, I'm running into issues with inverting the 'S' matrix due to the DEFAULT_ZERO_TOLERANCE value in algebra.c. I propose this value be changed from 1e-8 to something like 1e-12, but I haven't yet tested this in my work to see how stable this would be...

I'm not sure what the best way to iterate over the forked version I posted, but I'm happy to work with you on updating my proposed changes to something that can be added to the main repository.

Replying to your initial comment.

I agree that having the linear code call some subset of code meant for the EKF (or vice-versa). I also agree that it's not super efficient that each rc_matrix function basically frees and re-allocates memory, even if the matrices are appropriately sized. I'm not quite at the point yet where I care about runtime of this code, but when I move to fly this on my drone, you'll probably see me around again pushing to update the matrix functions.

I don't have any specific literature to point to on using Kalman filters with different rates. It was actually just taught to me this way in classes. This often means you'll have sequences of prediction steps with no corrections, and sometimes (less often) means you'll do many corrections before you do another prediction (e.g. if barometer and GPS have new measurements in the same iteration)

beagleboard / librobotcontrol

kalman.c should have separate prediction and correction steps #185