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$

1 Answer 1

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$
2
  • $\begingroup$ Makes more sense now, thank you! $\endgroup$
    – Anemone
    Nov 16, 2018 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$ Nov 16, 2018 at 11:24

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .