Skip to main content
1 of 3

assume we are tracking a vehicle with a radar/lidar. The radar/lidar is placed some where to observe the vehicle. the state of the vehicle is

$$ \mathbf{x}=\begin{bmatrix} x \\ v \end{bmatrix},P=\begin{bmatrix} cov(x,x) && cov(x,v) \\ cov(v,x) && cov(v,v) \end{bmatrix} $$

from the radar/lidar, we can read the time of echo signal, not the distance, but the time. In order to connect the state vector and the measurement, we need the measure matrix $H$. In this case,

$$ H=\begin{bmatrix} 1/c && 0 \end{bmatrix} $$

where $c$ is the light speed.

thus, the measurement is about time, which is

$$\begin{eqnarray} \mu_{k+1}&=&H_k \mathbf{x}_k=\begin{bmatrix} 1/c && 0 \end{bmatrix}\begin{bmatrix} x_k \\ v_k \end{bmatrix}=x_k/c \\ \Sigma_{k+1}&=&H_k P_k H_k^T=\begin{bmatrix} 1/c && 0 \end{bmatrix}\begin{bmatrix} \text{cov}(x_k,x_k) && \text{cov}(x_k,v_k) \\ \text{cov}(v_k,x_k) && \text{cov}(v_k,v_k) \end{bmatrix}\begin{bmatrix} 1/c \\ 0 \end{bmatrix}=\text{cov}(x_k,x_k)/c^2\\ \Sigma_{k+1}&=&=\text{var}(x_k/c) \end{eqnarray}$$