# Kalman filter continues to update position even though speed is zero

Background
I set up a conventional Kalman filter that makes use of smartphone GPS only (no inertial sensors). That is, it uses the position, doppler speed, and course, in order to create positions that represent a more accurate track.

When the device is stationary (i.e. the smartphone is placed on a table), the GPS drifts, and the doppler speed reports 0 m/s, as expected.

Problem and expected outcome
Given that the doppler speeds are all 0, I expect that the Kalman filter would "dismiss" subsequent position measurements and retain the original position measurement.

Details

• I convert GPS coordinates from geodetic coordinates to NED (Cartesian) coordinates, $$p_x, p_y$$, relative to an initial coordinate
• $$\psi$$ is course
• $$\sigma_h^2=h_{\tt accuracy}^2$$, where $$h_{\tt accuracy}$$ is the horizontal accuracy, in meters, and represents 68% probability that the position is within its radius
• $$\sigma_s^2=s_{\tt accuracy}^2$$, where $$s_{\tt accuracy}$$ is the speed accuracy, in meters/second, and represents 68% probability that the speed is within its range
• $$H=I$$

Here are the Kalman filter equations, step by step:

$$F=\begin{bmatrix}1&0&\Delta t&0\\0&1&0&\Delta t\\0&0&1&0\\0&0&0&1\end{bmatrix} \tag{1}$$
$$\hat x^-=F\hat x\tag{2}$$
$$Q=\begin{bmatrix}\sigma_h^2&0&\sigma_s^2&0\\0&\sigma_h^2&0&\sigma_s^2\\0&0&\sigma_s^2&0\\0&0&0&\sigma_s^2\end{bmatrix} \tag{3}$$
$$P=FPF^T+Q \tag{4}$$
$$R=\begin{bmatrix}\sigma_h^2&0&\sigma_s^2&0\\0&\sigma_h^2&0&\sigma_s^2\\0&0&\sigma_s^2&0\\0&0&0&\sigma_s^2\end{bmatrix} \tag{5}$$
$$K=PH^T(HPH^T+R)^{-1} \tag{6}$$
$$z=\begin{bmatrix}p_x\\p_y\\s\cos(\psi)\\s\sin(\psi)\end{bmatrix} \tag{7}$$
$$\hat x^+=\hat x^-+K(z-H\hat x^-) \tag{8}$$
$$P^+=P^--KHP^- \tag{9}$$

Question
If $$\sigma_h^2$$ is large (poor accuracy) and $$\sigma_s^2$$ is low (good accuracy), I expect that the predicted position will not change, but it continues to be influenced by the measurement and drift. Why is that?

First, you err in using the square root of accuracy in your covariance matrices. These should be squared. If your positional accuracy is $$\sigma_h$$ and your velocity accuracy is $$\sigma_s$$, then your measurement matrix should be

$$\mathbf R = \begin{bmatrix} \sigma_h^2 & 0 & 0 & 0 \\ 0 & \sigma_h^2 & 0 & 0 \\ 0 & 0 & \sigma_v^2 & 0 \\ 0 & 0 & 0 & \sigma_v^2 \\ \end{bmatrix}$$

Second, your $$\mathbf Q$$ matrix is telling the Kalman filter that the position moves a distance with a deviation of $$\sigma_h$$ each step, and its velocity changes with a deviation of $$\sigma_v$$ each step.

So you're telling the filter "hey, this phone is moving around in this huge and unpredictable way every single time step". As a consequence, at each time step the filter is going to put as much or more credence in the measurement as it does in its filtered position and velocity.

If you want the filter to track when you're moving and settle when you put it on a table, you either need GPS/IMU fusion, or you need to "tell" it (by setting the $$\mathbf Q$$ matrix very small) that it's sitting still when its on the table, and (by setting the $$\mathbf Q$$ matrix high) that it's moving when it's not on the table.

Figuring out how to do that, absent of full GPS/IMU fusion, I leave to you.

• I concur that the accuracies in $Q$ and $R$ should be squared. Thank you! I edited the question to reflect your correction and make it easier to read. May 27 at 18:35
• Why did you put $0$'s in $R_{13}$ and $R_{24}$? May 27 at 19:30
• If the measurement is what I think it is, it's only corrupted by its own variance, not some other channel's. May 27 at 21:22
• I see. Yes, I think that $R_{13}$ and $R_{24}$ would be 0 because speed has no correlation with position in regards to measurement. However, $Q_{13}$ and $Q_{24}$ would be non-zero because their values affect the prediction, as you can see in the state transition matrix, $F$. Is my reasoning correct? May 27 at 21:59
• Possibly non-zero, possibly not. What clue do you have for the value $\mathbf Q$? What a-priori knowledge do you have of the motion of the cell phone? May 28 at 4:05