Kalman filer for estimating velocity

The goal is to use a Kalman filter to estimate velocity from noisy position measurements. I am attempting to implement a version of the filter used in the example on the Wiki page Kalman Example for predicting velocity. However, attempts have so far been unsuccessful.

I believe my confusion is tuning the processes noise matrix $Q$ using the so called $G$ matrix and the acceleration scale factor $\sigma^2_a$ where $$G = \left[\begin{matrix} \frac{\Delta t^2}{2} \\ \Delta t\end{matrix} \right]$$

and

$$Q = GG^T \sigma^2_a$$

In my simple simulation, the position is defined as $$p(t) = A\cos(2\pi f t - \phi)$$ With $A = 0.5$ $f=2$ and $\phi = 0$, the implied velocity signal is $$v(t) = \frac{dp(t)}{dt}= -\pi f\sin(2\pi f t - \phi)$$

The position is sampled at a rate $F_s = 15 kHz$ with noise $\eta$ added of variance $\sigma^2_\eta = 10^{-4}$ to generate the observation signal $z(t)$.

In the filter $$F = \left[ \begin{matrix} 1 & \Delta t \\ 0 &1 \end{matrix} \right]$$ $$H = \left[ \begin{matrix} 1 & 0 \\ 0 & 0 \end{matrix} \right]$$ $$R = \left[ \begin{matrix} \sigma^2_\eta & 0 \\ 0 & 1 \end{matrix} \right]$$ $$P_0 = \left[ \begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix} \right]$$

If $\sigma^2_a$ is the variance of the acceleration $a(t)$, where $$a(t) = \frac{dv(t)}{dt} = -2 \pi^2 f^2 cos(2\pi f t - \phi)$$

then $\sigma^2_a = 1.2 \cdot 10^4$. This values seems unusually large. The resulting filter outputs are shown here:

Any suggestions would be greatly appreciated for tuning the parameters correctly. Thank you.

• In the wikipedia example, a is assumed to be normal distributed. In your example, this is not at all the case, but you have a deterministic a which does not follow a Gaussian distribution. Hence, the change of the acceleration cannot really be considered as noise and the filter is not meaningful. However, I cannot see, why $\sigma_a$ should be 12000, I'd calculate it to be $(2\pi^2 f^2)^2/2=3117$ – Maximilian Matthé Nov 21 '16 at 22:10
• Thanks for your input Maximilian. You are correct on the variance calculation.I was trying to use the sinusoid as a test signal. I tried a random walk as the input and it appears to have solved he problem. Thank you, – John M Nov 22 '16 at 13:30