【組合導航代碼ECL】狀态預測函數predictStates.m

寫在前面的的話

最近學習組合導航，要對ECL代碼有進一步的了解，暫時以此作為筆記

目錄

• • 寫在前面的的話
  • 主函數
  • • 計算步驟如下：
    • 三個全局變量
    • 全局變量的初始化
    • 減去ＩＭＵ傳感器偏差（速度、角度變化值）
    • 改正變化的速度 for rotation and skulling
    • 改正coning 誤差和地球自傳
    • • 地球自轉
      • coning 誤差
      • 将改正轉換到body坐标系下
    • 四元素預測
    • 計算旋轉矩陣T
    • 轉換速度變化值至導航坐标系
    • 儲存三個全局變量
    • 小結
    • 疑問

主函數

function [states, correctedDelAng, correctedDelVel]  = PredictStates( ...
    states, ... % previous state vector (4x1 quaternion, 3x1 velocity, 3x1 position, 
                     3x1 delAng bias, 3x1 delVel bias)
    delAng, ... % IMU delta angle measurements, 3x1 (rad) 變化的角度觀測值是用來計算四元數的中間值
    delVel, ... % IMU delta velocity measurements 3x1 (m/s) 變化的速度值是用來計算速度與位置的中間值
    dt, ... % accumulation time of the IMU measurement (sec) IMU測量的累積時間
    gravity, ... % acceleration due to gravity (m/s/s) 重力
    latitude) % WGS-84 latitude (rad) 次元 用來剪出地球自傳速度
 
           

計算步驟如下：

1. 先計算出改正的速度變化值與角度變化值
2. 通過改正後的角度變化值計算四元素預測值
3. 通過改正後的速度變化值計算導航坐标系下的位置速度預測值

三個全局變量

Δ a n g ( k − 1 ) \Delta_{ang}(k-1) Δang​(k−1)

Δ v e l ( k − 1 ) \Delta_{vel}(k-1) Δvel​(k−1)

[ T ] B N ( k − 1 ) [T]_B^N(k-1) [T]BN​(k−1)

全局變量的初始化

if( Δ a n g ( k − 1 ) \Delta_{ang}(k-1) Δang​(k−1)|| Δ v e l ( k − 1 ) \Delta_{vel}(k-1) Δvel​(k−1)|| [ T ] B N ( k − 1 ) [T]_B^N(k-1) [T]BN​(k−1)為空):

　　 Δ a n g ( k − 1 ) = Δ a n g \Delta_{ang}(k-1)=\Delta _{ang} Δang​(k−1)=Δang​

　　 Δ v e l ( k − 1 ) = Δ v e l \Delta_{vel}(k-1)=\Delta _{vel} Δvel​(k−1)=Δvel​

　　 [ T ] B N ( k − 1 ) = Q u a t 2 T B N ( Q u a t ( k − 1 ) ) [T]_B^N(k-1)=Quat2T_B^N(Quat(k-1)) [T]BN​(k−1)=Quat2TBN​(Quat(k−1))

減去ＩＭＵ傳感器偏差（速度、角度變化值）

Δ a n g ( k − 1 ) = Δ a n g ( k − 1 ) － Δ a n g b i a s ( k − 1 ) \Delta_{ang}(k-1)=\Delta_{ang}(k-1)－\Delta_{ang}bias(k-1) Δang​(k−1)=Δang​(k−1)－Δang​bias(k−1)

Δ v e l ( k − 1 ) = Δ v e l ( k − 1 ) − Δ v e l b i a s ( k − 1 ) \Delta_{vel}(k-1)=\Delta _{vel}(k-1)-\Delta_{vel}bias(k-1) Δvel​(k−1)=Δvel​(k−1)−Δvel​bias(k−1)

改正變化的速度 for rotation and skulling

公式參考 《A sculling digital computation algorithm》式25

ecl代碼中将改正注釋，并未改正

改正coning 誤差和地球自傳

地球自轉

當我們知道次元與地球自轉速度時，便可改正地球自傳對NU方向的影響

Δ a n g E a r N = 0.000072921 ∗ c o s ( l a t i t u d e ) ∗ d t \Delta_{ang}Ear_N=0.000072921 * cos(latitude) * dt Δang​EarN​=0.000072921∗cos(latitude)∗dt

Δ a n g E a r E = 0 \Delta_{ang}Ear_E=0 Δang​EarE​=0

Δ a n g E a r U = 0.000072921 ∗ s i n ( l a t i t u d e ) ∗ d t \Delta_{ang}Ear_U=0.000072921 * sin(latitude) * dt Δang​EarU​=0.000072921∗sin(latitude)∗dt

coning 誤差

公式參考《A new strapdown attitude algorithm》式11

ecl代碼将其注釋并未改正

将改正轉換到body坐标系下

Δ a n g c o r = Δ a n g − ( [ T ] B N ) T ∗ Δ a n g E a r \Delta_{ang}cor=\Delta_{ang}-([T]_B^N)^T*\Delta_{ang}Ear Δang​cor=Δang​−([T]BN​)T∗Δang​Ear

四元素預測

将改正的旋轉角度資訊轉換為四元素

Δ Q u a t = R o t a 2 Q u a t ( Δ a n g C o r ) \Delta_{Quat}=Rota2Quat(\Delta_{ang}Cor) ΔQuat​=Rota2Quat(Δang​Cor)

Q u a t ( k ) − = Q u a t ( k − 1 ) ∗ Δ Q u a t Quat(k)^-=Quat(k-1)*\Delta_{Quat} Quat(k)−=Quat(k−1)∗ΔQuat​

四元素的乘法需要再研究

對四元數進行歸一化

Q u a t ( k ) − = Q u a t ( k ) − ∣ Q u a t ( k ) − ∣ Quat(k)^-=\frac{Quat(k)^-}{|Quat(k)^-|} Quat(k)−=∣Quat(k)−∣Quat(k)−​

四元素的預測完成(公式詳見Process and Observation Moder.pdf)

計算旋轉矩陣T

T B N ( k − 1 ) − = Q u a t 2 T B N ( Q u a t ( k ) − ) T_B^N(k-1)^-=Quat2T_B^N(Quat(k)^-) TBN​(k−1)−=Quat2TBN​(Quat(k)−)

為何不使用平滑後的四元素來計算旋轉矩陣

轉換速度變化值至導航坐标系

Δ v e l N E D = T B N ∗ Δ v e l C o r + g ∗ d t \Delta_{vel}NED=T_B^N*\Delta_{vel}Cor+g*dt Δvel​NED=TBN​∗Δvel​Cor+g∗dt

V e l N E D ( k ) − = V e l N E D ( k − 1 ) + Δ v e l N E D Vel_{NED}(k)^-=Vel_{NED}(k-1)+\Delta_{vel}NED VelNED​(k)−=VelNED​(k−1)+Δvel​NED

P o s N E D ( k ) − = P o s k − 1 + 1 / 2 ∗ d t ∗ ( V e l N E D ( k − 1 ) + V e l N E D ( k − 1 ) − ) Pos_{NED}(k)^-=Pos_{k-1}+1/2*dt*(Vel_{NED}(k-1)+Vel_{NED}(k-1)^-) PosNED​(k)−=Posk−1​+1/2∗dt∗(VelNED​(k−1)+VelNED​(k−1)−)

儲存三個全局變量

Δ a n g ( k − 1 ) = Δ a n g \Delta_{ang}(k-1)=\Delta _{ang} Δang​(k−1)=Δang​

Δ v e l ( k − 1 ) = Δ v e l \Delta_{vel}(k-1)=\Delta _{vel} Δvel​(k−1)=Δvel​

[ T ] B N ( k − 1 ) = T B N [T]_B^N(k-1)=T_B^N [T]BN​(k−1)=TBN​

小結

1. Δ a n g \Delta_{ang} Δang​經過四個改正：系統偏差改正，地球自傳改正，coning改正(未執行)，rotation and skuling 改正（未執行）
2. Δ v e l \Delta_{vel} Δvel​經過一個改正：系統偏差改正

疑問

1. 為什麼角度變化值改正時有兩個量未改正？
2. 既然旋轉矩陣不高效，為什麼在代碼中一直在用 [ T ] B N [T]_B^N [T]BN​來旋轉角度，直接使用四元素來的會更加高效。
3. 為何不使用平滑後的四元素來計算旋轉矩陣
4. 本預測函數為何隻對四元素、位置速度進行預測，未對其他狀态量進行預測