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