EKF model&realization

REF： https://pythonrobotics.readthedocs.io/en/latest/modules/localization.html#unscented-kalman-filter-localization

Filter design

In this simulation, the robot has a state vector includes 4 states at time $t$ .

\textbf{x}_t=[x_t, y_t, \theta_t, v_t]

x, y are a 2D x-y position, $\theta$ is orientation, and v is velocity.

In the code, "xEst" means the state vector. code

And, $P_t$ is covariace matrix of the state,

$Q$ is covariance matrix of process noise,

$R$ is covariance matrix of observation noise at time $t$

The robot has a speed sensor and a gyro sensor.

So, the input vecor can be used as each time step

\textbf{u}_t=[v_t, \omega_t]

Also, the robot has a GNSS sensor, it means that the robot can observe x-y position at each time.

\textbf{z}_t=[x_t,y_t]

The input and observation vector includes sensor noise.

In the code, "observation" function generates the input and observation vector with noise code

Motion Model

The robot model is

\dot{x} = vcos(\phi)$$$$ \dot{y} = vsin((\phi)$$$$ \dot{\phi} = \omega

So, the motion model is

\textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_t

where

$\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}$

$\begin{equation*} B= \begin{bmatrix} cos(\phi)dt & 0\\ sin(\phi)dt & 0\\ 0 & dt\\ 1 & 0\\ \end{bmatrix} \end{equation*}$

$dt$ is a time interval.

This is implemented at code

Its Jacobian matrix is

$\begin{equation*} J_F= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv}\\ \frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} \end{equation*}$

$\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v sin(\phi)dt & cos(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}$

Observation Model

The robot can get x-y position infomation from GPS.

So GPS Observation model is

\textbf{z}_{t} = H\textbf{x}_t

where

$\begin{equation*} B= \begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0& 0\\ \end{bmatrix} \end{equation*}$

Its Jacobian matrix is

$\begin{equation*} J_H= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \end{bmatrix} \end{equation*}$