一種基於對偶四元數的慣導系統傳遞對準建模方法與流程

2023-06-04 02:27:56

本發明涉及航空航天系統信息處理的技術領域，具體涉及一種基於對偶四元數的慣導系統傳遞對準建模方法。

背景技術：

慣性導航系統是一套複雜的高精度機電綜合系統，歷經了平臺式慣性導航系統和捷聯式慣性導航系統，其中早期的平臺式慣導系統的姿態信息是由陀螺力矩為主的萬向支架構建的穩定平臺直接提供，而捷聯式慣導系統(捷聯式慣導系統)將慣性組件(Inertial Measurement Unit，IMU)直接固連在載體上，利用計算機平臺替代物理平臺，具備了體積小、成本低和可靠性高等特點。但是，捷聯慣性導航系統對IMU組件的性能提出了更高的要求，必須根據陀螺儀輸出的角速度計算並維持一個數學平臺，在此數學平臺上由加速度計輸出積分獲得速度和位置信息。因此，捷聯慣性導航方法在捷聯式慣導系統中具有非常重要的作用。

在捷聯式慣性導航系統中導航計算機的主要任務是實現角速度積分計算獲取載體姿態信息，即姿態解算，利用獲取的姿態信息將加速度變換到合適的導航坐標系，再進行加速度積分計算獲取載體的速度信息，進而對速度積分獲取載體位置信息，這兩步也稱為導航解算。為了保證導航方法誤差與慣性組件引入的誤差相比足夠小以致可以忽略不計，上述三個積分過程必須採用高精度的數值積分方法，如選用四階以上的Runge-Kutta法才能滿足捷聯式慣導系統微分方程的數值積分精度要求，以此來降低轉動的不可交換性帶來的負面影響，如圓錐(Coning)效應、划船(Sculling)效應和捲軸(Scrolling)效應等影響，也可以說捷聯慣性導航方法的發展過程就是一個尋求高動態環境下的高精度數值積分方法的發展史。

捷聯式慣性導航系統不同於平臺式慣導系統的重要特點就是以計算機實現數學平臺替代物理平臺，數學平臺根據IMU組件感測的數據信息計算和描述坐標系間的轉換關係，這種轉換關係中有三個轉動自由度和三個平移自由度。通常的坐標轉換關係採用方向餘弦法(DCM)，利用DCM表示轉動，向量表示平移運動，它需要九個未知元素，計算量較大；而四元數法有四個元素，計算量小、精度較高，目前大多採用四元數法實現載體姿態轉動的最簡潔表達方式。但是，這些傳統的姿態描述方法都把載體旋轉運動和平移運動割裂開來，實際上Charles定理表明任何剛體運動都可以通過繞某個軸的旋轉和沿相同軸的平移運動實現。因此，這種旋轉和平移組合運動很像是一種螺旋運動，可以用螺旋變換或者螺旋運動來描述一般性剛體運動。1873年Clifford提出了對偶數(Dual number)概念，並構建了對偶四元數代數(Dual Quaternion)來描述螺旋變換，其特點是對偶四元數把剛體的旋轉與平移運動統一考慮，數學表達直觀明了。

同時，20世紀90年代Branets把對偶四元數代數應用於慣性導航系統的領域，首次論述了捷聯式慣性導航系統理論分析中對偶四元數代數建模的可行性，並勾畫了利用對偶四元數描述捷聯式慣性導航系統中坐標系轉換的基本框架。實際上利用對偶四元數開展捷聯式慣性導航系統建模研究還帶來一個優勢，就是可以實現捷聯式慣性導航系統的無平臺計算。利用對偶四元數建立捷聯式慣導系統模型的過程中，基於推力坐標系T、引力坐標系I和位置坐標系U來實現，擯棄了以前物理平臺或者數學平臺的不利影響，這對於現代的無平臺慣導系統理論研究將會開闢一條新的途徑。

本發明基於對偶四元數描述方法，在推力坐標系、引力坐標系和位置坐標系基礎上研究一種新型的對偶四元數描述的捷聯式慣導系統傳遞對準模型算法。通過查閱已發表文獻可知利用對偶四元數建立慣導系統導航方法和捷聯式慣導系統的初始對準模型方法都已經出現了，但是對於現代導航系統在載體運動情形下的傳遞對準過程的建模研究還沒有相關文獻資料，更多的是利用四元數、修正羅德裡格斯參數或者歐拉角法實現傳遞對準系統建模研究分析。基於前人文獻資料的分析研究，本發明採用對偶四元數法構建傳遞對準模型算法具有較強的創新性，並且與傳統的姿態描述方法建模獲得的傳遞對準模型算法相比，傳遞對準的對偶四元數模型計算量小，計算效率高，並且脫離了捷聯平臺的影響，能夠有效擴展對偶四元數在慣導系統中的應用，也開闢了捷聯慣導系統模型研究的新途徑。傳遞對準是解決艦載機捷聯式慣導系統在艦船載體運動情況下海上對準問題的主要方法，它是利用艦船慣導裝置導航參數與機載慣導系統相應導航參數匹配計算，利用濾波算法估計機載慣導系統失準角等導航參數，進而對機載慣導系統導航參數初始化。

技術實現要素：

為了解決上述技術問題，本發明提出一種基於對偶四元數的慣導系統傳遞對準建模方法，基於對偶四元數描述，在推力坐標系、引力坐標系和位置坐標系下構建傳遞對準模型，脫離了捷聯平臺影響；利用對偶四元數來統一描述載體姿態轉動和平移運動過程，擺脫了傳統方法的旋轉和平移分離計算帶來的捷聯計算中圓錐誤差和划船誤差的影響，有效提高了的計算精度和計算效率。

為了達到上述目的，本發明的技術方案是：一種基於對偶四元數的慣導系統傳遞對準建模方法，其步驟如下：

步驟一：設定艦船主慣導系統的載體坐標係為m，艦載機子慣導系統的標稱載體坐標係為s，艦載機子慣導系統的計算載體坐標係為s』；利用對偶四元數定義主慣導系統載體系m與子慣導系統的標稱載體坐標系s間的標稱對偶四元數向量利用對偶四元數定義主慣導系統載體坐標系m相對於子慣導系統計算載體系s』間的計算對偶四元數向量其中，表示主慣導系統載體坐標系m相對於導航坐標系n在子慣導系統的標稱載體坐標系s中的投影四元數，表示對偶算子，r表示主慣導系統與子慣導系統標稱載體坐標系間的杆臂矢量，表示主慣導系統載體坐標系m相對於導航坐標系n在子慣導系統的計算載體坐標系s′中的投影四元數，r'表示主慣導系統相對於子慣導系統的計算載體坐標系s′間的杆臂矢量；

步驟二：在傳遞對準過程中，標稱子慣導系統相對於主慣導系統是靜止的，構造主慣導系統載體系m與子慣導系統的標稱載體坐標系s間的標稱對偶四元數向量誤差微分方程：把標稱對偶四元數向量誤差微分方程作為系統模型方程的一部分參與系統狀態變量的最優濾波計算；

步驟三：考慮主慣導系統載體系m與子慣導系統的標稱載體坐標系s旋量間的關係，主慣導系統載體系m與子慣導系統的標稱載體坐標系s相對旋轉與平移的對偶四元數旋量其中，表示主慣導系統載體坐標系m相對於導航坐標系n的對偶四元數旋量，表示子慣導系統的標稱載體坐標系相對於導航坐標系n的對偶四元數旋量，εs表示子慣導系統中的陀螺零漂移誤差量；

步驟四：構造主慣導系統載體系m與子慣導系統的計算載體坐標係為s』間的標稱對偶四元數向量誤差微分方程：其中，表示子慣導系統的計算載體坐標系相對於主慣導系統的旋量在導航坐標系中的投影，(.)*表示向量的逆；

步驟五：聯合加速度計誤差微分方程：陀螺儀誤差微分方程：主慣導系統和子慣導系統間的杆臂矢量微分方程：構建主慣導系統和子慣導系統間的傳遞對準系統誤差模型方程：

其中，ε表示系統中陀螺儀隨機漂移的三維矢量，表示系統中加速度計的三維零漂移誤差量；連同三維杆臂矢量和八維的標稱對偶四元數微分方程，系統模型中需要添加017×17的零矩陣；

步驟六：考慮線速度誤差和旋轉角速度誤差，構造主慣導系統與子慣導系統傳遞對準過程中的對偶四元數旋量的觀測方程：

其中，表示觀測模型中的計算子慣導系統相對於主慣導系統在導航坐標系中觀測到的對偶旋量，表示計算子慣導系統相對於主慣導系統的旋量在導航系中的投影的觀測量；Bω表示系統陀螺儀和加速度計的偏差的對偶量，ηω表示系統速度旋量的觀測噪聲；

步驟七：利用Kalman濾波方法求解傳遞對準系統誤差模型方程和觀測旋量方程，獲得系統狀態變量：標稱姿態失準角、計算姿態失準角、加速度計和陀螺儀參數的計算。

所述主慣導系統載體系m與子慣導系統的計算載體坐標係為s』間的標稱對偶四元數向量誤差微分方程的構建方法是：

根據對偶四元數的微分表達式計算對偶四元數向量的微分方程表達式：其中，表示主慣導系統與計算子慣導系統間的旋量，且

其中，表示主慣導系統相對於導航坐標系的旋轉角速度在計算子慣導系統中的投影，表示主慣導系統相對於導航坐標系的線速度在計算子慣導系統中的投影的微分，表示主慣導系統相對於導航坐標系的旋轉四元數在計算子慣導系統中的投影的共軛；

主慣導系統相對於導航坐標系的線速度在計算子慣導系統中的投影的微分可表示為其中表示主慣導系統的線加速度，表示計算子慣導系統的線加速度，表示杆臂矢量的加速度，根據慣導系統速度方程得：

其中，fm表示主慣導系統的比力，表示主慣導系統的旋轉四元數，表示主慣導系統的地球自轉角速度，表示主慣導系統相對於地球坐標系e的旋轉角速度，Vm表示主慣導系統的速度，gm表示導航坐標系中的重力加速度；

主慣導系統和子慣導系統間的杆臂誤差速度表達為其微分方程為：

其中，表示主慣導系統相對於導航坐標系的旋轉角速度，rm表示杆臂矢量，表示主慣導系統相對於地球坐標系的旋轉角速度的微分；

那麼可以獲得主慣導系統與子慣導系統速度誤差表達式：

其中，是的逆，表示主子慣導系統間的相對速度誤差在導航坐標系中的投影，表示計算子慣導系統相對於導航系的旋轉四元數，fs表示子慣導系統的測量比力，表示主慣導系統相對於計算子慣導系統的旋轉四元數，主子慣導系統間的旋轉四元數，表示主慣導系統相對於計算子慣導系統的旋轉四元數的逆，表示地球自轉角速度，表示地球自轉角速度在導航坐標系中的投影，表示主慣導系統相對於地球坐標系的旋轉角速度，表示計算子慣導系統和主慣導系統間的相對線速度在導航系中的投影，rn表示導航系中的杆臂矢量，表示子慣導系統的加速度計測量偏差；

針對主慣導系統的對偶四元數以及子慣導系統的對偶四元數的微分方程分別為：

其中，表示主慣導系統的對偶四元數的微分，表示主慣導系統的對偶四元數，表示主慣導系統的旋量；

主慣導系統與子慣導系統的旋轉對偶四元數間的關係：其中，表示子慣導系統的旋量，表示主子慣導系統間的對偶四元數的共軛，表示主子慣導系統間的相對旋量在導航系中的投影，表示主子慣導系統間的對偶四元數；

利用方向餘弦矩陣表達旋轉角速度與間的關係為：其中，表示主子慣導系統間的方向餘弦矩陣；對主慣導系統的對偶四元數分解得：對其兩邊同時求導整理得：

其中，表示主慣導系統相對於導航坐標系的對偶四元數在計算子慣導系統的投影，表示計算子慣導系統相對於導航系的對偶四元數，表示主慣導系統的對偶四元數的微分，表示計算子慣導系統的對偶四元數的微分，表示主慣導系統相對於導航坐標系的對偶四元數；

結合主慣導系統與子慣導系統的姿態對偶四元數微分方程獲得：

其中，表示標稱子慣導系統相對於導航系的旋量，表示主慣導系統相對於導航系的旋量；

考慮主慣導系統與子慣導系統間的旋量間的關係，主慣導系統與子慣導系統間的相對旋轉與平移的對偶四元數旋量為：

從而得那麼其逆為：

獲得主慣導系統與子慣導系統的姿態平移對偶四元數誤差方程為：

所述加速度計誤差微分方程和陀螺儀誤差微分方程的構建方法是：

系統觀測旋量表達的是主子慣導系統與慣導系統間的相對旋轉與平移誤差對偶四元數向量，其顯性表達式為：

其中，是主慣導系統與子慣導系統間的旋轉誤差矢量，且：表示主子慣導系統間的相對線速度誤差，且：

子慣導系統敏感到的角速度和加速度可分別表示為：

其中，表示能被主慣導系統敏感到的載體角速度在子慣導系統慣性系i中的投影，ε為陀螺零偏誤差，為主慣導系統敏感到的載體加速度在子慣導系統慣性系i中的投影，為載體轉動引起的子慣導系統杆臂加速度，為加速度計的零偏誤差，ng和na為隨機噪聲；

待標參數誤差為常值誤差，隨機噪聲為零均值不相關白噪聲，則有：

所述主慣導系統與子慣導系統傳遞對準過程中的對偶四元數旋量的觀測旋量方程的構建方法是：採用角速度+線速度的匹配模式獲取子慣導系統解算的線速度，主慣導系統提供的經過剛性杆臂補償後的速度，將其與子慣導系統輸出速度相減獲得速度差作為線速度觀測量；取角速度誤差觀測量為可表達為：

相應的線速度誤差可表達為：

其中，表示主慣導系統與計算子慣導系統的速度誤差，表示子慣導系統的輸出速度，表示主慣導系統的輸出速度，表示主慣導系統的姿態矩陣，表示主慣導系統相對於地球系的旋轉角速度，表示加速度計的偏差；

則系統觀測旋量可統一表示為：

其中，表示計算子慣導系統相對於主慣導系統的旋轉角速度在導航系中的投影，表示計算子慣導系統相對於主慣導系統的線速度在導航系中的投影；Bω＝bω+εbv，是角速度觀測量的偏差，表示系統線速度的偏差；ηω＝ηω+εηv，表示系統角速度觀測噪聲，表示系統線速度觀測噪聲，和均為高斯白噪聲。

本發明利用對偶四元數構造主慣導系統和子慣導系統間的標稱對偶四元數來描述子慣導系統載體系相對於主慣導系統載體系的旋轉和平移運動，構造計算對偶四元數來描述主慣導系統載體系相對於子慣導系統計算載體系的旋轉和平移運動，再通過推理主慣導系統相對於子慣導系統的相對旋轉和平移運動的旋量表達式、傳遞對準的標稱對偶四元數微分方程，聯合系統加速度計參數誤差方程和陀螺儀誤差微分方程，獲得傳遞對準系統的對偶四元數誤差方程；利用加速度計的線速度和陀螺儀的旋轉角速度構造系統旋量對偶四元數觀測量方程，從而獲得一種基於對偶四元數的傳遞對準系統誤差模型，經由kalman濾波迭代計算獲得子慣導系統的初始標定參數操作。本發明擺脫了旋轉和平移分離計算帶來的圓錐誤差和划船誤差的影響，有效提高了計算精度和計算效率。

附圖說明

為了更清楚地說明本發明實施例或現有技術中的技術方案，下面將對實施例或現有技術描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發明的一些實施例，對於本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其他的附圖。

圖1為本發明主慣導系統與子慣導系統見傳遞對準原理圖。

圖2為本發明標稱姿態失準角計算誤差數據曲線。

圖3為本發明計算姿態失準角計算誤差數據曲線。

圖4為本發明四元數誤差模型的標稱姿態失準角計算誤差數據曲線。

圖5為本發明四元數誤差模型的計算姿態失準角計算誤差數據曲線。

具體實施方式

下面將結合本發明實施例中的附圖，對本發明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發明一部分實施例，而不是全部的實施例。基於本發明中的實施例，本領域普通技術人員在沒有付出創造性勞動前提下所獲得的所有其他實施例，都屬於本發明保護的範圍。

本發明首先論述對偶四元數的基本概念及其運算規則，接著利用對偶四元數建立傳遞對準在推力坐標系下的推力速度微分方程、在引力坐標系下的引力速度微分方程以及在位置坐標系下的位置坐標微分方程；再對這三組微分方程計算求解獲得捷聯慣導系統的姿態角、速度和位置量，經由坐標轉換獲得艦載機載體在當地地理坐標系下的導航初始參數；最後對偶四元數的解算精度做出說明與論證。最後給出系統初始參數進行仿真驗證算法的有效性與計算效能。

一種基於對偶四元數的慣導系統傳遞對準建模方法，其步驟如下：

步驟一：設定艦船主慣導系統的載體坐標係為m，艦載機子慣導系統的標稱載體坐標係為s，艦載機子慣導系統的計算載體坐標係為s』；利用對偶四元數定義主慣導系統載體系m與子慣導系統的標稱載體坐標系s間的標稱對偶四元數向量利用對偶四元數定義主慣導系統的載體坐標系m相對於子慣導系統計算載體系s』間的計算對偶四元數向量其中，表示主慣導系統m相對於導航坐標系n在子慣導系統的標稱載體坐標系s中的投影四元數，表示對偶算子，r表示主慣導系統與標稱子慣導系統間的杆臂矢量，表示主慣導系統的載體坐標系m相對於導航坐標系n在計算子慣導系統s′中的投影四元數，r'表示主慣導系統相對於子慣導系統的計算載體坐標系s′間的杆臂矢量。

對偶四元數是一種元素為對偶數的特殊四元數，其定義為其中Q稱為實部，Q'稱為對偶部，它們都是四元數，對偶部中的是對偶單位數，滿足且對偶四元數的範數定義為：

其中，表示對偶四元數的共軛，其表達為：若對偶四元數的實部非零，那麼對偶四元數的逆存在，且表示為對於單位對偶四元數來說其逆等於對偶四元數的共軛，即對偶四元數作為一類特殊的四元數，它完全繼承了四元數的性質，但是其有一些獨特的運算規則。

對偶四元數加法定義：

對偶四元數與一個標量相乘的乘法定義：

對偶四元數間的乘法定義：

且其交換律滿足

對偶四元數的點乘乘法定義：

對偶四元數叉乘乘法定義：

且其交換律滿足

對偶四元數與矩陣的乘積定義：設矩陣M∈R8×8，且那麼矩陣與一個對偶四元數的乘積為

對偶四元數的圈乘定義:

在本發明中艦船主慣導系統的載體坐標係為m，艦載機子慣導系統的標稱載體坐標係為s，艦載機子慣導系統的計算載體坐標係為s』。在傳遞對準過程中，主慣導系統是一個無誤差信源的基準系統，在艦船行進過程中，艦船主慣導系統的載體坐標系m與艦載機子慣導系統的標稱載體坐標系s之間的相對位置是固定不變的，那麼可以定義一個標稱對偶四元數表示子慣導系統相對於主慣導系統的相對旋轉與平移運動：其中，是主慣導系統與子慣導系統間的旋轉四元數，矢量r表示主慣導系統與子慣導系統間的位移矢量。同時主慣導系統相對於子慣導系統的計算載體坐標系定義一個計算對偶四元數那麼

步驟二：構造主慣導系統載體系m與子慣導系統的標稱載體坐標系s間的標稱對偶四元數向量誤差微分方程：

本發明利用微小擾動攝動法來推導傳遞對準系統的誤差方程。在傳遞對準過程中一般認為，標稱子慣導系統相對於主慣導系統是靜止的，標稱對偶四元數可以認為是一個常值，因此其微分可表示為：把標稱對偶四元數向量誤差微分方程作為系統模型方程的一部分參與系統狀態變量的最優濾波計算。

步驟三：考慮主慣導系統載體系m與子慣導系統的標稱載體坐標系s旋量間的關係，主慣導系統載體系m與子慣導系統的標稱載體坐標系s相對旋轉與平移的對偶四元數旋量其中，表示主慣導系統相對於導航坐標系n的對偶四元數旋量,表示標稱子慣導系統相對於導航坐標系的對偶四元數旋量，εs表示子慣導系統中的陀螺零漂移誤差量。

主慣導系統與子慣導系統的旋轉對偶四元數間具有這樣的關係：若是利用方向餘弦矩陣表達則有旋轉角速度與間有可以對主慣導系統的對偶四元數分解獲得並且對其兩邊同時求導整理獲得：

結合主慣導系統和子慣導系統的姿態對偶四元數微分方程獲得

可以考慮主慣導系統與子慣導系統的旋量間的關係，定義一個表達主子慣導系統間的相對旋轉與平移的對偶四元數旋量其表達式為

步驟四：構造主慣導系統載體系m與子慣導系統的計算載體坐標係為s』間的標稱對偶四元數向量誤差微分方程：其中，表示計算子慣導系統相對於主慣導系統的旋量在導航坐標系中的投影，(.)*表示向量的逆。

根據對偶四元數的微分表達式定義計算對偶四元數向量的微分方程表達式：其中，表示主慣導系統與子慣導系統間的旋量，也是一個對偶四元數，其一般表達式為：

其中，表示主慣導系統相對於導航坐標系的旋轉角速度在計算子慣導系統中的投影，表示主慣導系統相對於導航坐標系的線速度在計算子慣導系統中的投影的微分，表示主慣導系統相對於導航坐標系的旋轉四元數在計算子慣導系統中的投影的共軛。

其中，主慣導系統相對於導航坐標系的線速度在計算子慣導系統中的投影的微分可表示為其中表示主慣導系統的線加速度，表示計算子慣導系統的線加速度，表示杆臂矢量的加速度。根據慣導系統速度方程可得到：

其中，fm表示主慣導系統的比力，表示主慣導系統的旋轉四元數，表示主慣導系統的地球自轉角速度，表示主慣導系統相對於地球坐標系e的旋轉角速度，Vm表示主慣導系統的速度，gm表示導航坐標系中的重力加速度。

主慣導系統和子慣導系統間的杆臂誤差速度可表達為其微分方程為：

其中，表示主慣導系統相對於導航坐標系的旋轉角速度，rm表示杆臂矢量，表示主慣導系統相對於地球坐標系的旋轉角速度的微分。

那麼可以獲得主慣導系統與子慣導系統速度誤差表達式：

其中，是的逆，表示主子慣導系統間的相對速度誤差在導航坐標系中的投影，表示計算子慣導系統相對於導航系的旋轉四元數，fs表示子慣導系統的測量比力，表示主慣導系統相對於計算子慣導系統的旋轉四元數，主子慣導系統間的旋轉四元數，表示主慣導系統相對於計算子慣導系統的旋轉四元數的逆，表示地球自轉角速度，表示地球自轉角速度在導航坐標系中的投影，表示主慣導系統相對於地球坐標系的旋轉角速度，表示計算子慣導系統和主慣導系統間的相對線速度在導航系中的投影，rn表示導航系中的杆臂矢量，表示子慣導系統的加速度計測量偏差。

針對主慣導系統的對偶四元數以及子慣導系統的對偶四元數的微分方程分別為：

其中，表示主慣導系統的對偶四元數的微分，表示主慣導系統的對偶四元數，表示主慣導系統的旋量。

主慣導系統與子慣導系統的旋轉對偶四元數間的關係：其中，表示子慣導系統的旋量，表示主子慣導系統間的對偶四元數的共軛，表示主子慣導系統間的相對旋量在導航系中的投影，表示主子慣導系統間的對偶四元數。

利用方向餘弦矩陣表達旋轉角速度與間的關係為：其中，表示主子慣導系統間的方向餘弦矩陣；對主慣導系統的對偶四元數分解得：對其兩邊同時求導整理得：

其中，表示主慣導系統相對於導航坐標系的對偶四元數在計算子慣導系統的投影，表示計算子慣導系統相對於導航系的對偶四元數，表示主慣導系統的對偶四元數的微分、表示計算子慣導系統的對偶四元數的微分，表示主慣導系統相對於導航坐標系的對偶四元數。

結合主慣導系統與子慣導系統的姿態對偶四元數微分方程獲得：

其中，表示標稱子慣導系統相對於導航系的旋量，表示主慣導系統相對於導航系的旋量。

考慮主慣導系統與子慣導系統間的旋量間的關係，主慣導系統與子慣導系統間的相對旋轉與平移的對偶四元數旋量為：

從而得那麼其逆為：

因此，獲得主慣導系統與子慣導系統的姿態平移對偶四元數誤差方程為：

步驟五：聯合加速度計誤差微分方程：陀螺儀誤差微分方程：主慣導系統和子慣導系統間的杆臂矢量微分方程：構建主慣導系統和子慣導系統間的傳遞對準系統誤差模型方程：

其中，ε表示系統中陀螺儀隨機漂移的三維矢量，表示系統中加速度計的三維零漂移誤差量；連同三維杆臂矢量和八維的標稱對偶四元數微分方程，系統模型中需要添加017×17的零矩陣陣。

系統觀測旋量表達的是主子慣導系統與慣導系統間的相對旋轉與平移誤差對偶四元數向量，其顯性表達式為：

其中，是主慣導系統與子慣導系統間的旋轉誤差矢量，且：表示主子慣導系統間的相對線速度誤差，且

子慣導系統敏感到的角速度和加速度可分別表示為：

其中，表示能被主慣導系統敏感到的載體角速度在子慣導系統慣性系中的投影，ε為陀螺零偏誤差，為主慣導系統敏感到的載體加速度在子慣導系統慣性系中的投影，為載體轉動引起的子慣導系統杆臂加速度，為加速度計的零偏誤差，ng和na為隨機噪聲；

待標參數誤差為常值誤差，隨機噪聲為零均值不相關白噪聲，則有：

主慣導系統與子慣導系統間傳遞對準過程中杆臂矢量rn是不變的，其微分方程為

步驟六：考慮線速度誤差和旋轉角速度誤差，構造主慣導系統與子慣導系統傳遞對準過程中的對偶四元數旋量的觀測旋量方程：

其中，表示觀測模型中的計算子慣導系統相對於主慣導系統在導航坐標系中觀測到的對偶旋量，表示計算子慣導系統相對於主慣導系統的旋量在導航系中的投影的觀測量，其表達式為表示計算子慣導系統相對於主慣導系統的旋轉角速度在導航系中的投影，表示計算子慣導系統相對於主慣導系統的線速度在導航系中的投影、Bω表示系統陀螺儀和加速度計的偏差的對偶量，可表示為Bω＝bω+εbv，是角速度觀測量的偏差，表示系統線速度的偏差、ηω＝ηω+εηv表示系統速度旋量的觀測噪聲，表示系統角速度觀測噪聲，表示系統線速度觀測噪聲，我們都假設為高斯白噪聲。

本發明採用角速度+線速度的匹配模式獲取子慣導系統解算的線速度，主慣導系統提供的經過剛性杆臂補償後的速度，將其與子慣導系統輸出速度相減獲得速度差作為線速度觀測量；取角速度誤差觀測量為可表達為：

相應的線速度誤差可表達為：

其中，表示主慣導系統與計算子慣導系統的速度誤差、表示子慣導系統的輸出速度、表示主慣導系統的輸出速度、表示主慣導系統的姿態矩陣、表示主慣導系統相對於地球系的旋轉角速度、表示加速度計的偏差。

則系統觀測旋量可進一步整理為：

步驟七：利用Kalman濾波方法求解傳遞對準系統誤差模型方程和觀測旋量方程，獲得系統狀態變量：標稱姿態失準角、計算姿態失準角、加速度計和陀螺儀參數的計算。

根據前面的子慣導系統狀態參數對偶四元數傳遞對準模型的推理過程，可以總結獲得系統過程模型方程為

和對偶數旋量的系統觀測旋量方程為

下面考慮迭代遞推過程中系統噪聲和觀測噪聲方差矩陣計算問題。

針對對偶速度的系統觀測旋量方程：

其中，對偶速度是一個對偶數向量，且是主慣導系統和子慣導系統的旋轉角速度誤差的觀測量在導航坐標系中的投影，其中，是主子慣導系統在導航坐標系中投影的線速度誤差觀測量。針對前面已經推導出的主慣導系統和子慣導系統的角速度誤差和線速度誤差的表達形式，將其觀測量偏差以及噪聲整理為Bω和ηω，且並且是角速度誤差量的偏差，其中，是線速度觀測量的偏差；其中，ηω表示角速度誤差噪聲，ηv表示線速度誤差噪聲，且假設其為高斯白噪聲向量，且具有噪聲方差矩陣

在迭代遞推計算過程中，每一步迭代計算後都需要考慮對偶四元數的強制規範化問題。對於對偶四元數的實部部分，根據對偶四元數定義式：則其實部部分的規範化計算式：

相應的對偶四元數的對偶部分量部分Q'可以採用計算表達式

利用對偶四元數的強制規範化計算結果參與下一次迭代計算，從而保證迭代濾波計算的穩定性。

根據前面系統狀態方程，可知系統狀態變量為狀態變量的初始值都設為0，利用艦船三軸搖擺運動模型仿真艦載機慣導系統，艦船縱搖、橫搖和航向搖擺幅度分別為6°，5°和7°；搖擺頻率分別為0.05，0.10，和0.05Hz；初始角分別設為0°，0°和90°；艦船東向和北向初始速度取值為10m/s，艦船所在地理位置設為北緯45.60°，東經124.75°。為了驗證本發明傳遞對準系統對偶四元數誤差模型的魯棒性，初始姿態誤差角設為15°，45°和60°，陀螺常值漂移為5°/h，隨機漂移為0.01°/h；加速度計初始偏差為0.09g，隨機漂移為0.004g,忽略慣性組件安裝誤差以及刻度因子誤差，線速度測量誤差0.01m/s。從而對傳遞對準系統的對偶四元數誤差模型展開仿真計算，獲得仿真結果如圖2-5所示。本發明展示了主慣導系統與子慣導系統間的標稱姿態失準角、計算姿態失準角在對偶四元數誤差模型迭代遞推計算的結果，並且和四元數誤差模型進行對比驗證。由圖2-5可得，本發明計算的標稱姿態失準角誤差數據比較穩定，能夠快速收斂於零軸附近，計算效能明顯優於傳統的四元數模型算法。

本發明綜合利用對偶四元數構建的傳遞對準系統的對偶四元數誤差模型，以及利用線速度和角速度構建的觀測旋量方程，可以採用Kalman濾波方法，最終獲得系統狀態變量，如標稱姿態失準角、計算姿態失準角以及加速度計和陀螺儀參數的估計計算，完成傳遞對準系統參數的初始標定操作。

本發明說明書中未作詳細描述的內容屬於本領域專業技術人員所公知的現有技術。以上所述僅為本發明的較佳實施例而已，並不用以限制本發明，凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的保護範圍之內。