Multi sensor fusion of imu/ optical mouse / wheel encoder (nonlinear Kalman filter)

Analysis of each sensor

imu

For planar mobile robots （ Like a robot sweeping the floor ）,IMU It only needs the yaw angle of the gyroscope （YAW）, There is time drift error in the yaw angle of the gyroscope , It is generally divided into system drift error （offset） And random time drift error .

Wheel encoder

There's nothing to say , The two wheeled differential robot has a corresponding motion model , The cumulative error gradually increases with factors such as slipping . It is necessary to calibrate the wheel diameter and the center distance between the two wheels , There are many calibration methods （todo: Update three calibration methods ）.

Photoelectric mouse sensor

It can be measured xy Offset , It's important to put it in different positions and methods of the robot .

Kalman fusion filter

prediction model

The model is the model of two differential wheels , Here we need to do a partial derivative , Calculate the Jacobian matrix , State transition matrix F, That is, it reflects nonlinearity , Others can follow the flow of Kalman Filter Algorithm .

Starting model variance P0 Set to 0.1× Unit matrix , Process noise Q From small to large .

Observation model

todo

Make a state transformation matrix H come out

Measure noise covariance R According to static / Linear motion and circular motion are counted , Figure out ×9 times （ according to 3sigma Gaussian noise model to deal with ）