I have some accelerometer sensors that gather data every second:
AC.X AC.Y AC.Z
9234 -0.98 -0.10 0.03
9235 -0.98 -0.10 0.03
9236 -0.98 -0.10 0.03
9237 -0.98 -0.10 0.03
9238 -0.98 -0.10 0.03
9239 -0.98 -0.10 0.03
The GPS data set gets captured in longer intervals:
X1 X2 X3 X4 X5 X6 X7 X8 X9 X10 X11 X12 X13 X14 X15 X16 X17 X18
81 $GPGGA 181553.00 4105.86029 N 08754.49966 W 1 06 1.90 00208 M -033 M *53
133 $GPGGA 181554.00 4105.86052 N 08754.49954 W 1 05 2.04 00208 M -033 M *54
185 $GPGGA 181555.00 4105.86002 N 08754.49950 W 1 06 1.90 00208 M -033 M *59
237 $GPGGA 181556.00 4105.85944 N 08754.49889 W 1 06 1.90 00207 M -033 M *58
289 $GPGGA 181557.00 4105.85952 N 08754.49877 W 1 07 1.30 00210 M -033 M *52
341 $GPGGA 181558.00 4105.85925 N 08754.49852 W 1 06 1.35 00209 M -033 M *56
I have been reading up how to use Kalman filters to integrate this data, and I am beginning to get a good understanding of how the algorithm works. Both data sets introduce different types of error (accelerometer data drifts over time, but is practically noiseless; GPS data has little drift but lots of noise), and the Kalman filter allows us to take advantage of these error profiles for better navigation data. However, I am still struggling to put it into code. I code in R and Python, but I am just looking for some general pseudo-code to get me started. Do you know any good resources or have experience with this sort of problem?
The kalman-filter is an algorithm based off previous data. It is easy to implement when you have predictable motion (for example a swinging pendulum). However for general purpose use can be difficult to implement correctly.
I have found it easier (and in some cases more reliable) to implement the simpler LLS filter (linear least squares) if all you are wanting to do is smooth out your GPS. Below is the python example from Wikipedia which considered three points in 2-d space, and I used as a starting point for my code:
import numpy as np
import matplotlib.pyplot as plt
x = np.random.rand(5,2)*10
a = np.matrix([ [1,x[0][0]], [1,x[1][0]], [1,x[2][0]] ])
b = np.matrix([ [x[0][2]], [x[1][3]], [x[2][4]] ])
yy = (a.T * a).I * a.T*b
xx = np.linspace(1,10,50)
y = np.array(yy[0]+yy[1]*xx)
plt.figure(1)
plt.plot(xx,y.T,color='r')
plt.scatter([x[0][0],x[1][0],x[2][0] ],[x[0][5],x[1][6],x[2][7] ])
plt.show()
If you are still interested in using the Kalman-filter this article is incredible useful in implementing a kalman-filter on an accelerometer in a dynamic system