Prepared

by

A.Kaviyarasu
Teaching Fellow
Department of Aerospace Engineering
Madras Institute Of Technology
Chromepet, Chennai
 A frame in which all Newton Law’s obeys.
 Inertial Frame is also called non accelerating
frame.
 X-axis points north.
 Y-axis points east.
 Z-axis points towards down.
 Inertial frame is also consider
as NED Frame.
 Note: Because the z-axis points
down the altitude above the ground is negative.
 Body frame is the coordinate system in which
the frame is aligned with body of the sensor.
 X-axis point out of the nose
 Y-axis points out right side
of the Fuselage
 Z-axis points out the bottom
of the Fuselage
Conversion from
inertial frame
to Body frame
  

 cos   sin   0 
 
RIv1      sin   cos   0 
 0 0 1 
 
  

 cos   0  sin   
 
Rv1     0
v2
1 0 
 sin   0 cos   
 
  

1 0 0 
 
Rv 2      0 cos    sin    
B

 0  sin    cos    
 
 RIB   ,   RvB2   Rvv12   RIv1  
 C C C S S 
 
RIB    ,    C S  S   C S CC  S  S S  C S  
 S  S  CC S  C S S   C S  CC 

 The rotation matrix for moving opposite direction
from body frame to the inertial frame.

RBI   ,   RIv1    Rvv12    RvB2  

  C C C S  S   C S S  S  CC S  
 
RBI   ,    C S CC  S  S S  C S S   C S  
  S 
 C S  C  C 
 The rategyro,accelerometer and magnetometer are aligned with
the body frame of vehicle.
 In order to get inertial frame data ,the sensor outputs are converted
from the body frame to the inertial frame.
 This can be accomplished by performing the matrix multiplication
RBI   ,  .
 The resultant matrix for converting Body frame angular rates (p,q,r)
. . .
into Euler angular rate (  , , )is
. 0  0 
 p 
     
 q   R B () 0   R B () R  ( ) .   R B () R  ( ) R ( ) 0 
  
 
 
 
  
.
 r  0
  0
   
   
1 0 0   cos   0  sin   
   
R ( )   0 cos    sin     R ( )   0 1 0 
 0  sin    cos      sin   0 cos   
   

RB ()  Identity Matrix

 . 

 p  1 0  sin( )   
 q    0 cos   . 
     sin    cos     
 r   0  sin    cos    cos     . 
 
 
Inverting the relation gives relationship between body rate and Euler rate.

.   
 
   p   1 sin    tan( ) cos    tan( )   p 
 
.
q    0  q 

   J    cos     sin      
.  r   sin    cos      r 

  0 
   cos    cos    
J is the rotational matrix
 .   

   p  q sin    tan( )  r cos    tan( ) 
.  
    q cos     r sin    

.  sin    cos    
   q r 
   cos   cos   
 This operation explains mathematically why
gimbal lock becomes a problem when using Euler
Angles. To estimate yaw, pitch, and roll rates,
gyro data must be converted to their proper
coordinate frames using the matrix J. But notice
that there is a division by in two places on the last
row of the matrix.
 When the pitch angle approaches +/- 90 degrees,
the denominator goes to zero and the matrix
elements diverge to infinity, causing the filter to
fail.
 Quaternion is a number system that extends
complex number.
 Used to represent the attitude of object in
3D space without Gimbal lock.
   
 cos 
2
 q0   

q 1 Vx sin 
   2 
 q2    
  Vy sin 2 
 q3   
  
Vz sin 2 
 
 q0   Rotation Angle in radians 
q 1  x Component of Rotation Axis 
   
 q2   y Component of Rotation Axis 
q  q0  q1i  q2 j  q3k    
 q3   z Component of Rotation Axis 
Normalisation    | q | = q0  q1  q2  q3
2 2 2 2

Inverse ----> q  q '1

|q|

q 0  q1  q2  q3  1 ---- for unit quaternion.

2 2 2 2

1
q  q'
 Addition of two vector
vector1  vector 2  w1  w2,( x1  x2)i,( y1  y2) j,( z1  z 2)k
 Subraction of two vector
vector1  vector 2  w1  w2,( x1  x2)i,( y1  y 2) j,( z1  z 2)k
 Multiplication of two vector
vector1  w1  x1i  y1 j  z1k
vector 2  w2  x2i  y 2 j  z 2k
. i j k

i -1 k -j
j -k -1 i
k j -i -1
vector1* vector 2   w1w2  x1x 2  y1 y 2  z1z 2 
+  w1x 2  x1w2  y1z 2  z1y 2  i
+  w1y 2  x1z 2  y1w2  z1x 2  j
+  w1z 2  x1y 2  y1x 2  z1w2  k
  w1 x1 y1 z1 
 -x1 w1 z1 - y1
Matrix1     w2 x2 y2 z2  
- y1 -z1 w1 x1 
 
 - z1 y1 - x1 w1


 w2 x2 y2 z 2 
 -x2 w2 -z 2 y2 
Matrix 2     w1 x1 y1 z1  
- y2 z 2 w2 -x2 
 
 - z 2 - y 2 x 2 w2 
 q 1
 q '  w  xi  yj  zk

Matrix 2 becomes
 w2 - x2 - y2 - z 2 
 x2 w2 z 2 - y2 
 w1 x1 y1 z1  
 y2 -z 2 w2 x2 
 
 z 2 y2 -x2 w2 

R q
i
b b
i   (Matrix2) 1
*(Matrix1)
  w2 -x2 - y 2 -z 2   w1 x1 y1 z1 
 x2 w2 z 2 - y 2   -x1 w1 z1 - y1 
Rib q   
b * 
 y2 -z 2 w2 x2  - y1 -z1 w1 x1 
i

   
 z 2 y 2 - x2 w 2
  - z1 y1 - x1 w1

w  w1  w2 y  y1  y 2
x  x1  x 2 z  z1  z 2

 w2 +x 2  y 2  z 2 0 0 0 
 
 0 w2  x 2  y 2  z 2 2xy 2wz 2xz  2wy 
Rb
q   
b

2 yz 2wx
i i

 0 2 xy  2wz w2  x 2  y 2  z 2 
 0 2xz  2wy 2 yz  2wx w2  x 2  y 2  z 2 

The rotation from the inertial frame to the body frame can
be performed using matrix multiplication Rib  qib  .

 
Rib qib  qq '
Rotation Matrix

Vb    Vi
Rib qib
Rotation Matrix
 Practical
approach for using
Quaternion
 Quaternion from attitude (q[0],q[1],q[2],q[3] from , , )

 Normalize Quaternion
 Update Quaternion with rates
 Normalize Quaternion
 Attitude from Updated Quaternion
           
cos 2  cos  cos  + sin   sin   sin   
   2 2 2 2  2 
 q[0]               
q[1]  sin  2  cos  cos  - cos  sin   sin   
    2 2 2 2 2
 q[2]              
  cos 2  sin   cos  + sin   cos  sin   
 q[3]     2 2 2 2  2 
           
cos 2  cos 
2
sin   - sin  
2 2
sin 
2
cos  
2

norm  q[0]  q[1]  q[2]  q[3]
2 2 2 2
  q[0]   q[0] 
q[1]  q[1] 
     *[Bodyrate]* 
1
 q[2] 2  q[2]
   
q
 [3]
 q[3] 

 q[0]   0 -w1 -w2 -w3   q[0] 

q[1]   w1 0 w3 -w2  q[1] 
  *  1  *   * dt
 q[2] 2  w2 -w3 0 w1   q[2]
     
q
 [3]
 w3 w2 -w1 0   q[3] 

 q[0]   0 -w1* q[1] -w2* q[2] -w3* q[3]

q[1]   w1* q[0] 0 w3* q[2] -w2* q[3]
  *  1  * dt
 q[2] 2  w2* q[0] -w3* q[1] 0 w1* q[3] 
   
q
 [3]
 w3* q[0] w 2* q[1] -w1* q[2] 0 
  2(q[0]q[1]  q[2]q[3]) 
  tan 
1

 1  2 q[1]2
 2 q[2]2

  sin 1  2q[0]q[2]  2q[1]q[3]
 2(q[0]q[3]  q[1]q[2]) 
  tan 
1

 1  2q[2]  2q[3] 
2 2
 The main disadvantage of unit
quaternion's, however, is that they are
constrained to have unit length.
Particle
Filter
 Create a random particles around state particles
 Update state particles X[i] positions by its control
inputs
 Update measurement particles y[i] positions by its
control inputs
 Calculate Likelihood for every particles
 Resample Particles
 Show state Estimates
 Particle filter again classified into
measurement update and time update.