[發明專利]一種基于級聯卡爾曼濾波算法的水面艦船水平姿態測量方法

有效

• 申請號: 202011276612.5
• 申請日: 2020-11-16
• 公開（公告）號: CN112504275B
• 公開（公告）日: 2022-09-02
• 發明（設計）人: 趙玉新;李帥陽;奔粵陽;吳磊;李倩;周廣濤;臧新樂;魏廷梟;溫官昊
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01C25/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于級聯卡爾曼濾波算法的水面艦船水平姿態測量方法，其特征在于：步驟如下：

步驟1：對捷聯慣性導航系統的慣性測量元件進行充分預熱，并完成初始對準，使之進入導航工作狀態；

步驟2：將GPS給出的位置信息作為量測，利用間接法卡爾曼濾波器對實時解算的導航參數進行校正，獲得校正后的捷聯慣性導航系統的捷聯矩陣速度V₁ⁿ和位置P₁；

步驟3：利用步驟2中校正后的捷聯慣性導航系統的捷聯矩陣速度V₁ⁿ和位置P₁，根據比力方程對運載體機動引起的線加速度和哥氏加速度進行補償，得到加速度計在導航系的理論輸出f₁ⁿ；

步驟4：將步驟3中求得的f₁ⁿ投影到載體系，根據加速度計在載體系的實際輸出f^b，利用互補濾波獲得的陀螺積分后的姿態誤差獲得陀螺積分誤差Δω^b；

陀螺積分后的姿態誤差的計算為：

其中，f₁ⁿ為加速度計在導航系的理論輸出，f^b為加速度計載體系的實際輸出，為經卡爾曼濾波校正后的捷聯姿態矩陣，^T表示對矩陣轉置，| |為相應矢量模值；

步驟5：將步驟4中得到的陀螺積分誤差Δω^b作為量測量，再次利用卡爾曼濾波對已經校正過的捷聯慣性導航解算的捷聯矩陣進行校正，獲得校正后的捷聯姿態矩陣

2.根據權利要求1所述的一種基于級聯卡爾曼濾波算法的水面艦船水平姿態測量方法，其特征在于：所述步驟3加速度計在導航系的理論輸出f₁ⁿ的求取為：

其中，V₁ⁿ為經卡爾曼濾波校正后的速度，為相鄰兩個姿態解算時刻的速度變化率，為校正后的地球自轉角速度在導航系的投影，為運載體的水平速度引起導航系相當于地球坐標系的旋轉角速度，gⁿ為校正后的地球重力矢量在導航系的投影。

3.根據權利要求1或2所述的一種基于級聯卡爾曼濾波算法的水面艦船水平姿態測量方法，其特征在于：所述步驟5中量測量的選取，將陀螺積分誤差Δω^b作為捷聯慣性導航進行卡爾曼濾波時的量測量Z₂(t)，量測量Z₂(t)和對應量測矩陣H₂(t)分別為：