CHR-6dm Inertial Measurement Unit (IMU)

• Micro controller unit: ARM STM32 • Accelerometer: ADXL335

• Gyroscopes: LPR510AL, LY510ALH • Magnetometer: HMC5843
Euler Equations

  p  tan   q sin   r cos  

  q cos   r sin 
q sin   r cos 
 
cos 
 Kalman Filter

Klein, V., and Morelli, E. A., Aircraft System Identification, AIAA Education Series, 2006, pp. 75-94
 IMU CHR-6dm Specifications

Sensor reference Sensor bias Sensor alignment

 0   122  1 0 0
ALgyro   0 1 0 
accel   0  bgyro   106 
   0 0 1 
 6000   35 
1 0 0
 255 0 
ALaccel  0 1 0 
mag   0  baccel  0 
  0 0 1 
666  0 
 20 
   0.001218163 0 0 
bmag  22 CAL   0 0.001197028 0 
  mag  
 9   0 0 0.001296065
 Kalman Filter
1. Initialization
ˆ 180 az (1)
Compute initial roll and pitch angles  (1)   a tan
 ay (1)
ˆ 180 az (1)
 (1)   a tan
 ax(1)
ˆ (1)  0
1. Initialization
 0.01 0 0 
P   0 0.01 0 
 0 0 0.01
0.01 0 0 
Q   0 0.01 0 
 0 0 0.01
1000 0 0 
Raccel   0 1000 0 
 0 0 1000 
0.00001 0 0 
Rmag   0 0.00001 0 
 0 0 0.00001
2. Prediction
 (i )   (i  1)  T .
ˆ  ˆ 

 (i )   (i  1)  T .
ˆ  ˆ 

ˆ (i )  ˆ (i  1)  T .
 

where
  p  tan   q sin   r cos   (3a )
  q cos  r sin  (3b)
q sin   r cos 


cos
2. Prediction
State covariance estimation

P  P  T ( AP  PAT  Q )

where   p  tan   q sin   r cos   (3a )

  q cos   r sin  x  Ax  (3
Bbu)
q sin   r cos 
 
cos
 
2 2
 q cos  tan   r sin  tan  r cos  (tan   1)  q sin  (tan   1) 0 
 
A  r cos   q sin  0 0
 q cos  r sin  r cos  sin  q sin  sin  
   0
 cos  cos  cos 2  cos 2  
3. Acceleration Measurement Update

ˆ   ˆ   correction(1)
ˆ   ˆ   correction(2)
ˆ   ˆ   correction(3)
where

correction  L(a  aˆ )
Measured acceleration:

a  ALaccel (asensor  baccel )

 3. Acceleration Measurement Update
Predicted acceleration:

b
aˆ  C accel i
y  Cx

where accel  ALaccel accel  baccel 

b
C i

 0 az cos   ax cos sin   ay sin sin 


C   ax(sin  sin  cos  cos sin  )  ay (cos sin   cos  sin sin  )  az cos  cos  ) axcos cos sin - azsinsin + aycos sin sin -ax
 ax(cos sin - cos sinsin ) - ay(cos cos + sinsin sin ) - azcos sin axcos cos cos - azcos sin + aycos cos sin ax(cos

az cos   ax cos sin   ay sin sin  ay cos cos  ax cos  sin 
) axcos cos sin - azsin sin + aycos sin sin -ax(cos cos + sin sin sin ) - ay(cos sin - cos sin sin ) 
     + aycos cos sin ax(cos *sin - cos sin sin ) + ay(sin *sin + cos *cos *sin ) 
3. Acceleration Measurement Update
Kalman gain:

T T 1
L  PC ( Raccel  CPC )

State covariance estimation

P  ( I  LC ) P
4. Magnetometer Measurement Update
ˆ   ˆ   correction(1)
ˆ   ˆ   correction(2)
ˆ   ˆ   correction(3)
ˆ)
where correction  L( m  m
Predicted Magnetic Field: m̂  Rmag y  Cx

 cos sin 0
R    sin cos 0  
mag  ALmag mag  bmag 
 0 0 1 

0 0 my cos  mx sin 
C  0 0  mx cos  my sin 
0 0 0 
4. Magnetometer Measurement Update
Measured Magnetic Field:

m  ALmag (msensor  bmag )

Rotate measurement into vehicle-1 frame

m  Rm
4. Magnetometer Measurement Update
Compute Kalman gain L

T T 1
L  PC ( Raccel  CPC )
Compute new covariance

P  ( I  LC ) P
Guide to AHRS Interface
Guide to AHRS Interface
Guide to AHRS Programming