慣性導航誤差微分方程
在IMU慣性導航誤差分析建模中 一共有三個微分方程 分別對應姿態誤差微分方程 速度誤差微分方程和位置誤差微分方程
姿態誤差微分方程
n為東北天坐標系 b為機體坐標系 這里求出二者之間的轉換關系就可以求出IMU的姿態 這里旋轉關系用旋轉矩陣C表示 對這個旋轉矩陣求導：
\(\omega^b_{nb}\)為從n系轉到b系 並且這個旋轉在b系下的表示 這個不容易進行測量 所以將這個角速度進行拆分 最后得到：
上面的方程中有三個變量 這三個變量均含有誤差 下面對這三個帶有誤差的變量進行逐一分析
旋轉向量
頭上帶波浪線的表示這個變量包含了誤差
我們要求的就是n系和b系之間的旋轉矩陣 但是這個旋轉矩陣包含噪聲 現在我們做以下認為：
我們認為這個旋轉矩陣的誤差在n系上 帶誤差的 旋轉矩陣將b系旋轉到了n' 系而不是n系 所以需要一個n系和n’ 系的旋轉矩陣
從n系到n' 系 我們用旋轉矢量\(\phi\)來表示 旋轉矢量的方向為繞着軸旋轉軸的方向 大小或者模長為旋轉的角度
旋轉矢量其實包含三個元素 稱為失准角 在后面狀態轉移方程中要用到
有羅德里格斯公式：
因為旋轉矢量趨近於0 所以最后結果為
角速度 i 到 b
后面的誤差項其實就是IMU內參的誤差 展開為：
其中\(\varepsilon\)為陀螺儀的bias
角速度 i到 n
不是做導彈這種慣性導航的話 這兩項誤差太小 不作考慮
化簡結果
中間化簡過程就省略了 難度不是很大 展示最后化簡結果：
速度誤差微分方程
帶誤差的微分方程：
f為加速度
上面的公式中四個變量均有誤差 下面進行分解：
最后一頓帶 得到最后結果：
位置誤差微分方程
這個就比較簡單了 直接給出結果了：
狀態轉移方程
這里的狀態轉移方程可以理解為卡爾曼濾波中的狀態轉移方程
首先我們將上面三個誤差微分方程做進一步展開
首先是位姿誤差微分方程展開
位姿誤差微分方程展開
誤差微分方程為
展開：
這里的\(\phi\)是我們之前提到過的旋轉矢量 它包括三個方向的失准角
\(\omega\)為地球自轉角速度 \(L\)為緯度值
\(\varepsilon\)為b系下的測量值 需要轉到n系下
速度誤差微分方程展開
道理跟上面位姿誤差微分方程展開差不多 這里不做過多解釋
其中\(\nabla\)為加速度計的bias
位置誤差微分方程展開
過於簡單不做解釋
整合進狀態轉移方程中
狀態方程一般形式為：
狀態量為：
其中F矩陣為X狀態量各個元素之間的轉移關系
腦補一下 橫着是五個狀態的元素 豎着也是五個狀態的元素
例如\(F_{23}\)為速度誤差的導數和姿態誤差的關系
W為器件的噪聲：
\(\omega_g\)為陀螺儀的噪聲 \(\omega_a\)為加速度計的噪聲
這一部分是F中的后半部分 也是可以求導獲得的 因為噪聲是和bias以同樣的方式輸入進來的