Having trouble implementing an extremely rudimentary Kalman filter

After spending hour unsuccessfully debugging my Kalman filter code I decided to try the most rudimentary case I can think of and see if my issue persist. They do. Please help me figure out what I'm doing wrong.

This is the rudimentary case:

state variable: $x_t = \begin{bmatrix} X \\Y \\Z \end{bmatrix}$ & state covariance: $\Sigma_t = \begin{bmatrix} \sigma_{XX} & \sigma_{XY} & \sigma_{XZ} \\ \sigma_{YX} & \sigma_{YY} & \sigma_{YZ} \\ \sigma_{ZX} & \sigma_{YZ} & \sigma_{ZZ} \end{bmatrix}$

Now I'm feeding GPS position estimates and a covariance matrix to the KF as measurement inputs:

measurement: $z_t = \begin{bmatrix} X \\Y \\Z \end{bmatrix}$ & measurement covariance: $R_t$

Now the GPS data is a stream of measurements from a stationary receiver, so the position inputs are all close. What I want the filter to do is to use the knowledge that the receiver is fixed as a model to just reduce errors in the output a bit.

The state transition model:

$x_t = Ax_{t-1} + Bu_{t}$

There is no control variable so the last expression is omitted. Also since I'm using the information that the receiver is stationary $A = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$. So the filter essentially becomes this:

State projection step:

$\hat{x}_t = x_{t-1}$

$\hat{\Sigma}_t = \Sigma_{t-1}$

Kalman gain calculation:

$K_t = \hat{\Sigma}_t \space (\hat{\Sigma}_t+R_t)^{-1}$

and finally the state update step:

$x_t = \hat{x}_t + K_t \space (z_t - \hat{x}_t)$

$\Sigma_t = (I - K_t) \hat{\Sigma}_t$

These are the outputs I'm getting:

$X$ at each time step:

Variance of $X$ at each time step ($\sigma_{XX}$):

I was expecting something close to a horizontal line for X and a covariance that asymptotically approached a constant value. What could be the cause of this? I included the plots hoping someone may know what may cause such a behavior.

EDIT: In this rudimentary model I did not include any process noise. I did try adding some process noise to see if it had any effect and it did not.

• What do the measurements $z$ look like? The scale of $x$ is $\sim 10^8$, while the scale of $\sigma_x^2$ is $\sim 10$ ... usually it would be the other way around (e.g. even if just from the squared units). Are you coding your own Kalman filter? This is fine, but if you are using a standard platform (e.g. Matlab, Python, R) I would recommend having an established package on hand to compare against (this will be invaluable for debugging). – GeoMatt22 Sep 22 '16 at 3:16
• You are sharing names for your state and observation vectors, you have strange notation for the size of your covariance matrices, and your kalman filter recursions are only true for special cases. And I can't tell if these special cases are applying, because you never write down your model. I doubt I could say anything useful without more information, ...you might have more luck on stackoverflow, where you can paste specific code. Otherwise I'd edit this up a bit – Taylor Sep 22 '16 at 4:00
• @GeoMatt22, yes I am coding my own Kalman filter. Checking a standard platform sounds like a good idea. The scale of z is about $\sim 10^6$. I would have expected the $\sigma_x^2$ to be about $50 \sim 100$... I should probably look into that. I may be calculating the covariance incorrectly. Thanks. – somerandomdude Sep 22 '16 at 5:58
• @Taylor I made some edits. Hopefully this helps. The model was actually in there but I clarified it a bit more. It's just so simplistic that it's easy to miss. – somerandomdude Sep 22 '16 at 6:11