Extended Kalman Filter measurement residual computation

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$$.

$$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).$$
• @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. – Kwin van der Veen Nov 16 '18 at 11:24