0
$\begingroup$

I am trying to follow the computation of EKF presented in this paper http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=C9CB210A45F0D7ED5CA7DE174F1A5490?doi=10.1.1.681.8390&rep=rep1&type=pdf (p.16-18), with the only difference that my system state does not include the acceleration, so it is a $4x1$ matrix:

$$x_x(t) = \begin{bmatrix} x(t) \\ v_x(t) \\ y(t) \\ v_y(t) \end{bmatrix} $$ where $x(t)$ and $y(t)$ are the cartesian coordinates and $v_x(t)$ and $v_y(t)$ are the velocity components.

I have problem computing the State Estimate Update step (eq. 17 in the paper), where the measurement residual needs to be calculated. According to the paper, the State Estimate Update is:

$$\hat{x}_x(k^{+}) = \hat{x}_x(k^{-}) + K(k)[z_{z}(k)-h(\hat{x}_x(k^{-}))]$$

where $$z_z(k) = \begin{bmatrix} z_x(k) \\ z_y(k) \end{bmatrix}, $$ where $z_x(k)$ and $z_y(k)$ are the measurements of $x$ and $y$ positions and $$ h(x_x(k)) = \begin{bmatrix} x(k) &0 &0 &0 \\ 0 &0 &y(k) &0 \end{bmatrix}. $$

$K(k)$ is the Kalman Gain, which in my case is a $4x2$ matrix.

My question is, how do I subtract $h(\hat{x}_x(k^{-}))$ which is a $2x4$ matrix from $z_z(k)$, which is a $2x1$ matrix? If I am not mistaken, the result of $K(k)[z_{z}(k)-h(\hat{x}_x(k^{-}))]$ needs to be a $4x1$ matrix so that I can later add it to the previous estimate $\hat{x}_x(k^{-})$, which is also $4x1$.

$\endgroup$
0
$\begingroup$

I suspect it is a mistake and the actual equation should have been

$$ h(x(k)) = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} x(k). $$

In this case equation $(9)$ would also make more sense. So in your case it would be

$$ h(x(k)) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} x(k). $$

$\endgroup$
  • $\begingroup$ Makes more sense now, thank you! $\endgroup$ – Anemone Nov 16 '18 at 11:14
  • $\begingroup$ @Anemone Also equation $(2)$ should not have a dot on top of $x$, so I would not trust everything immediately from that paper directly if I where you, because it might contain more mistakes. $\endgroup$ – Kwin van der Veen Nov 16 '18 at 11:24

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.