I have setup a sensor fusion problem to estimate a classical 2d position + orientation of a wheeled mobile robot in an embedded environment.
Sensor measurements include:
- IMU (6axis accl+gyro) @ 40Hz
- GPS (2d position) @1Hz
- Wheel odometry (linear v_x, angular omega_z) @ 10Hz
I do not know what is the best way to setup an EKF estimate loop when such samples are not updating synchronously.
These are the possibilities that come to my mind:
- Consider the filter (and thus model dynamics) timestep constant (arbitrarily fixed, let's say the fastest sensor's sampling rate, i.e. IMU @ 40Hz) and each loop do 1 predict + 3 model measurement updates with the current sensor sample stored (1 update for each sensor sample). This considers constant sensor samples between subsequent loops, effectively assuming constant ans synchronous sampling.
- Consider the filter (and thus model dynamics) timestep variable (maybe adding such timestep as control variable) and do 1 prediction + 1 update for each sensor sample update. Such timestep should be computed from subsequent sample timestamp difference from a sample measurement queue.
The second approach sounds more complex as it requires a prediction step for each incoming sample (besides dealing with all the timestamps computation witchcraft) but may be the most accurate as the filter updates only when we are triggered by sensor updates.
Thanks you so much for your support :)