Linear Kalman Filter for Attitude Estimation from Angular Rate and a Single Vector Measurement

Algorithm 1

Proposed SVO LKF.

Initialize:

,

,

,

whilee  no stop commands received  do

   Input:  ,

   Prediction:  

   Propagation:  

   Kalman Gain:  

   Estimation:  

   Update of Covariance:   

   Normalization:  

end while