1
$\begingroup$

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:

Figure 1

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

$\endgroup$
  • 1
    $\begingroup$ 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$ $\endgroup$ – Maximilian Matthé Nov 21 '16 at 22:10
  • $\begingroup$ 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, $\endgroup$ – John M Nov 22 '16 at 13:30

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.