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 .

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

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

And,  is covariace matrix of the state,

 is covariance matrix of process noise,

 is covariance matrix of observation noise at time 

　

The robot has a speed sensor and a gyro sensor.

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

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

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

So, the motion model is

where

 is a time interval.

This is implemented at code

Its Jacobian matrix is

 

Observation Model

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

So GPS Observation model is

where

Its Jacobian matrix is

 

Extented Kalman Filter

Localization process using Extendted Kalman Filter:EKF is

=== Predict ===

=== Update ===