I've been trying to implement a Kalman filter to get estimates of current heading (angle) of a vehicle by performing a sensor data fusion (GPS + IMU).
The GPS sensor provides heading information $\left<-180^o,180^o\right)$ with rather significant uncertainties ($\pm5^{o}$), while the IMU has a really small error ($\pm0.2^{o}$) but has a constant dynamic drift of $0.5^{o}/min$. IMU output data also covers the same range as GPS, i.e. $\left<-180^o,180^o\right)$
The IMU can be mounted in any orientation regarding magnetic north, thus initially the difference between IMU and GPS measurements can have every value in range $\left<0^o,360^o\right)$.
Thanks to the data fusion we can have IMU constancy and low noise corrected by GPS true heading.
The data fusion process is as follows:
+
IMU_angle ---------+----------+-------------------------------------------> fusion output
| | ^ -
| | |
| +---> process noise ------> kalman filter -------+
|+ ^
- v |
GPS_angle ---> heading diff ------> measurement noise --------+
After few simulations I got rather positive results but I have a problem with how to deal with circular data. When GPS and/or IMU oscillates around $\pm180^{o}$, the heading difference between consecutive samples can jump to big values (close to $360^o$) and Kalman does not adapt quickly (which is obvious).
How should I deal with such issue?
For the sake of clarity (pseudo-code):
Q = 0.013 # process noise covariance
R = 8.346 # measurement noise covariance
x = 0 # initial conditions
P = Q
heading_diff[i] = -gps_heading[i] + imu_heading[i])
# prediction phase
P = P + Q;
# correction phase
K = P / (P + R)
x = x + K * (heading_diff[i] - x)
P = (1 - K) * P;
fusion_output[i] = imu_heading[i] - x
```
not sure this is still relevant, but we deal with these kinds of problems in our recently published paper that allows you to apply a Kalman-filter like algorithm to circular problems (the circular Kalman filter [1] see example 5). Basically, if your data is circular, then conflicting observations (e.g. 180 deg apart) will lead to qualitatively very different behavior than using a "standard" Kalman filter - you can deal with this nonlinearity that by considering a vector addition in 2D on the level of the sufficient statistics. Specifically, this extreme case won't affect your current estimate, but will reduce your estimate's certainty, such that subsequent observations can make up for that "conflict".
[1] Kutschireiter, Rast & Drugowitsch. 2022. “Projection Filtering with Observed State Increments with Applications in Continuous-Time Circular Filtering.” IEEE Transactions on Signal Processing. https://doi.org/10.1109/TSP.2022.3143471. Publicly available version: https://arxiv.org/pdf/2102.09650.pdf