Skip to content

Fusing GPS, IMU and Encoder sensors for accurate state estimation.

License

Notifications You must be signed in to change notification settings

huihui308/GPS_IMU_Kalman_Filter

 
 

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

41 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

ExtendedKalmanFilter

EKF to fuse GPS, IMU and encoder readings to estimate the pose of a ground robot in the navigation frame.

Wikipedia writes: In the extended Kalman filter, the state transition and observation models need not be linear functions of the state but may instead be differentiable functions.

x_k = g(x_k), u_k-1 + w_k-1
z_k = h(x_k) + v_k

EKF step

Where w_k and v_k are the process and observation noises which are both assumed to be zero mean Multivariate Gaussian noises with covariance matrix Q and R respectively.

The function g can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, g and h cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian matrix) is computed.

At each time step, the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearizes the non-linear function around the current estimate.

Here we have a velocity sensor (encoders/GPS velocity), which measures the vehicle speed (v) in heading direction (psi), a yaw rate sensor (psi_dot) and an accelerometer which measures longitudinal velocity which both have to fused with the position (x & y) from the GPS sensor.

Ground robot model

References

  1. @balzer82 for his tutorials on Kalman Filters.
  2. Probabilistic Robotics by Thrun, Burgard, and Fox.

About

Fusing GPS, IMU and Encoder sensors for accurate state estimation.

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages

  • C++ 97.8%
  • C 1.4%
  • Other 0.8%