A 2D Range-Bearing tracking problem estimates the states of a vehicle
$$x=(x,y,v_x, v_y)$$
The transition function for the constant velocity model is (This is Euler’s Method)
$$x_k=f(x_{k-1,\Delta t})=\begin{cases}
x_{k-1}+v_{x_k}\Delta t \\
y_{k-1}+v_{y_k}\Delta t
\end{cases}$$
The observation vector is a range and bearing from some point
$$
\begin{matrix}
z_b=atan2(y,x)\\
z_r=\sqrt{x^2+y^2}
\end{matrix}$$
The Jacobian of the transition function is
$$\frac{\partial f}{\partial x}=\begin{bmatrix}
1 & 0 & \Delta t & 0\\
0 & 1 & 0 & \Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}$$
Which gives the Jacobian observation model of
$$\frac{\partial h}{\partial x}=\begin{bmatrix}
\frac{-y}{x^2+y^2} & \frac{1}{x(1+(\frac{y}{x})^2)} & 0 & 0\\
\frac{x}{\sqrt{x^2+y^2}} & \frac{y}{\sqrt{x^2+y^2}} & 0 & 0
\end{bmatrix}$$
Backlinks
Sources
- [1] “Writing an Extended Kalman Filter (EKF) with mrpt::bayes — MRPT 2.11.1 documentation.” Accessed: Oct. 21, 2023. [Online]. Available: https://docs.mrpt.org/reference/latest/tutorial-ekf.html