Pages

Friday, February 24, 2012

Kalman Filtering for Dummies - Part V

Two example will be given in this post. Note that all the examples are discrete state space in kinematic form. And all these systems are inputless.

Getting pitch from Accelerometer and Rate Gyroscope

The absolute pitch can be obtain from the accelerometers. Due to the effect of gravity on the ay and az direction, the absolute pitch can be obtained from atan2 function of ay and az value. But sometimes the absolute pitch is effected by noise from actual acceleration and deceleration. Therefore this is when the rate gyroscope comes in handy.

Therefore, we can design the state variables to be absolute pitch and rate of pitch. The state transition matrix is a simple kinematic matrix like previous parts. And since we can measure all the state variables, the output matrix is a size two identity matrix. The state equation and the output equation are as follow:




Getting the Robot Posture from Encoders, Accelerometer and Rate Gyroscope


There are several methods (model) to obtain the robot posture (x, y, and theta) from all the mentioned sensors. In this case, I would propose the reading from the rate gyroscope, accelerometer, and encoder with the sensors as placed, shown in figure above.

The problem is quite tricky because obtaining robot posture from encoder it not direct but via integration method. Besides, the robot posture information obtained is relative. In other words, the data obtain is not absolute; which implies that the data is prone to cumulative error. Kalman filter might help to reduce the cumulative error but will not solve it unless there is a sensor to obtain absolute position reading (i.e. GPS).

To solve this problem, I choose to use four state variables. The longitudinal velocity, longitudinal acceleration, ax, heading and heading rate, w. This will be a four dimensional Kalman filter. But since the longitudinal velocity and acceleration are orthonormal with heading and heading rate, the Kalman filter can be decoupled in to two separate Kalman filter. I will be sticking to the four dimensional Kalman.


Since all states variables can be measured, the output matrix is a size four identity matrix. The heading, theta can be obtain from integration from the encoders, heading rate, from the rate heading gyroscope, longitudinal velocity, vx, from the encoders and finally, the longitudinal acceleration from the accelerometer.

The robot posture is not obtained directly from the state variables but through some kind of matrix. Users can search for the kinematic equation for a robot posture on the internet.


In the next part, I will be showing how to obtain Kalman state equation and output equation for a dynamic system. This system will have an input, as opposed to the examples shown until now. Kalman filter in a dynamic system can be used together with linear quadratic regulator to solve a linear quadratic gaussian problem. This is a type of optimal controller in advance control system study.

Part I
Part II
Part III
Part IV
Part V
Part VI

No comments: