Naive Kalman filter for 3D position

Question

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

Answer 1

The thing you should keep in the back of your mind is that R Kalman published his seminal paper in 1962 (or there about). He was a Control Engineer, interested in real systems at a time prior to widely available high fidelity numerical multi-physics modeling. The kinematic models you contrast, are derived from $$ F=ma =m \frac{dv}{dt}. $$ and $m$ is often a constant point mass. Planes and rockets consume mass for propulsion so we can see early on that there will be neglected terms. $F$ should also include aerodynamic drag and for laminar flow, drag is roughly proportional $v^2$ which is not even close to the Navier-Stokes equations. So even a simple drag model is nonlinear and for a real time online algorithm a linear state space model is all he really had. The solution to these oversimplifications is noise and small step sizes and prediction feedback.

Noise can also be actual random phenomena. So a lot of non trivial modeling errors are lumped together in $$ \frac{d}{dt} \mathbf{x}(t)= \mathbf{A}\mathbf{x}(t) + \mathbf{B} \mathbf{u}(t) $$
where $\mathbf{u}(t)$ can be control forces, random forces, or psuedo modeling errors that we treat as noise. We also have a measurement set of equations that can also include noise, real or conceptual. You can stick with this form and use a Kalman-Bucy Filter or you can convert the continuous state space model to a difference equation which is typically used in what is a Kalman Filter. To do that you need to integrate the equation just given. I'm not going to go in detail because there are books and websites that show this but one start with calculating the matrix exponnential $$\mathbf{F}= \mathbf{e}^{\mathbf{A} \Delta t} $$. Some of your examples calculate the matrix exponential and some don't. You can often obtain a symbolic solutution for the matrix exponential or you can solve it numerically.

Getting back to your original question is that these models are simplifications of simplifications. There may be poor choices taken but there really isn't a single correct discrete time 3D state space model. some may have $x_{k+1}=x_{k} + \Delta t v_{k}$ and some might include $\frac{(\Delta t)^2}{2} a_k$. Real objects aren't point masses. Angular orientation matters. A lot of what is missing is treated as noise over small time steps. When the nonlinearity is more severe, we switch to the extended Kalman Filter. In the future, the state models will be real time multi-physics numerical routines.

The best model is the one that works best with you data.