Karman filter and attitude estimation Lin Zhong ELEC424, Fall 2010

Yaw, pitch, and roll angles

Inclination How z is represented in the sensor coordinate XYZ: S z

Orientation (attitude) How xyz is represented in the sensor coordinate XYZ: GS R=[ S x, S y, S z] T G a = GS R· S a

Accelerometer Acceleration Why is it not enough?

Gyroscope Angular velocity Why is it not enough?

Kalman filter What does it do? – Estimate the internal state x of a linear dynamic system from noisy measurements How does it estimate it? – Linearity of the system – Statistical properties of the system and measurement – Recursive (dynamic programming)

The target system The system evolves in discrete time steps – F k is the state transition model – B k is the control-input model – w k is the process noise, assumed to a zero mean multivariable normal distribution w k ~N(0, Q k )

The measurement The measurement (observation) of the state x k is a linear function of x k – H k is the measurement model – v k is the measurement noise, a zero mean multivariable normal distribution v k ~N(0, R k )

Discrete Kalman filter Two variables are updated at each stage (k) – : state estimation given measurements up to and including time k – : error covariance matrix (how accurate is)

Recursive estimation At time 0, and are known Given them at k-1, Predict and Update & Measurement residual Residual covariance Optimal Kalman gain

A gyroscope measures a 3D angular velocity plus an offset and white measurement noise in the sensor co-ordinate frame The spectrum of the gyroscope offset has a low cutoff frequency in comparison with the bandwidth of the kinematic signals that are to be measured

A 3D accelerometer measures acceleration minus gravity and a white noise component, all in the sensor co-ordinate frame The acceleration of a body segment in the global system can be described as low pass filtered white noise

Inclination estimated from gyroscope Strapdown integration GS R t-1 GS R t Remove offset

Inclination estimated from accelerometer Remove body acceleration GS R Predict Rotate S z A = S g t /| S g t |=

What assumptions can we make? Offset of gyroscope and accelerometer can be calibrated, automatically. Roll and pitch are small Body acceleration is small (if engines are controlled properly) Goal – Yaw should be constant – Roll and pitch should be small