I looked at posts that discusses 3D kalman filter. Kalman Filter to estimate 3D position of a node Help with Kalman Filter implementation for estimating 3D position
Both from my understanding, both person were attempting the same thing, which is calculating 3D position based on accelerator readings. Yet their process for setting up the Kalman filter is completely different.
My own method is also different
These are my equation based on what I learned here, it treats acceleration as the U vector http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf
Xk = AXk-1 + BUt
x_curr = x_prev + x_vel_prev * dt + 0.5*dt^2*x_acc
x_vel_curr = x_vel_prev + dt*x_acc
The sescond post seemed to have dropped the U vector completely similiar to this example http://campar.in.tum.de/Chair/KalmanFilter#Literature.
While the first post has a different process equation based on wiki and it uses Ga instead of Bu https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical:
X = FX_prev + Ga
This is what I did and it make sense I think:
X = [x,y,z,dx,dy,dz]
U = [ddx, ddy, ddz]
A = np.matrix([[1, 0, 0, dt, 0, 0],
[0, 1, 0, 0, dt, 0],
[0, 0, 1, 0, 0, dt],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]],
)
B = np.matrix([[0.5*(dt**2), 0, 0],
[0, 0.5*(dt**2), 0],
[0, 0, 0.5*(dt**2)],
[dt, 0, 0],
[0, dt, 0],
[0, 0, dt]])
H = np.matrix([[1,0,0,0,0,0],
[0,1,0,0,0,0],
[0,0,1,0,0,0]])
P = np.matrix([[10,0,0,0,0,0],
[0,10,0,0,0,0],
[0,0,10,0,0,0],
[0,0,0,100,0,0],
[0,0,0,0,100,0],
[0,0,0,0,0,100]])
Q = np.matrix([[1,0,0,0,0,0],
[0,1,0,0,0,0],
[0,0,1,0,0,0],
[0,0,0,1,0,0],
[0,0,0,0,1,0],
[0,0,0,0,0,1]])
R = np.matrix([[1,0,0],
[0,1,0],
[0,0,1]])
What I dont understand is, how come the previous 2 posts have completely different matrix? Is there a reason why? And is my approach correct?
Any input and suggestion is greatly appreciated