[發(fā)明專利]一種基于加性四元數(shù)的大方位失準(zhǔn)角線性對準(zhǔn)方法

有效

• 申請?zhí)枺?/td>: 201610835249.3
• 申請日: 2016-09-20
• 公開（公告）號(hào): CN106595701B
• 公開（公告）日: 2019-07-09
• 發(fā)明（設(shè)計(jì)）人: 王可東
• 申請（專利權(quán)）人: 南京喂啊游通信科技有限公司
• 主分類號(hào): G01C25/00
• 分類號(hào): G01C25/00
• 代理公司: 北京科迪生專利代理有限責(zé)任公司 11251
• 代理人: 成金玉;盧紀(jì)

1.一種基于加性四元數(shù)的大方位失準(zhǔn)角線性對準(zhǔn)方法，其特征在于步驟如下：

(1)選擇經(jīng)度誤差，緯度誤差，高程誤差，東向、北向和天向速度誤差，加性四元數(shù)誤差作為狀態(tài)變量，根據(jù)狀態(tài)方程和觀測方程，利用GPS觀測信息，通過卡爾曼濾波，進(jìn)行粗對準(zhǔn)，其中狀態(tài)方程由位置誤差方程、線性化的速度誤差方程和姿態(tài)誤差方程組成；

(2)在粗對準(zhǔn)期間，采用滑動(dòng)窗口法來進(jìn)行切換準(zhǔn)則判定，即設(shè)定固定長度的時(shí)間窗口，在窗口內(nèi)，直接由加性四元數(shù)計(jì)算當(dāng)前航向角收斂水平，并判斷航向角誤差是否全部滿足閾值條件；若滿足，則保存協(xié)方差陣，轉(zhuǎn)入精對準(zhǔn)；若不滿足，則繼續(xù)步驟(1)的粗對準(zhǔn)；

(3)延續(xù)步驟(1)的狀態(tài)變量及其位置誤差方程、姿態(tài)誤差方程和觀測方程，使用小角度下的線性化速度誤差方程，并將保存的協(xié)方差陣作為卡爾曼濾波的初始條件，進(jìn)行精對準(zhǔn)，使精對準(zhǔn)能快速進(jìn)入收斂狀態(tài)，至對準(zhǔn)達(dá)到預(yù)期水平。

2.根據(jù)權(quán)利要求1所述的基于加性四元數(shù)的大方位失準(zhǔn)角線性對準(zhǔn)方法，其特征在于：所述步驟(1)中，所述卡爾曼濾波為線性卡爾曼濾波。

3.根據(jù)權(quán)利要求1所述的基于加性四元數(shù)的大方位失準(zhǔn)角線性對準(zhǔn)方法，其特征在于：所述步驟(2)中的閾值條件為：表示計(jì)算得到的加性四元數(shù)，表示加性四元數(shù)誤差的估計(jì)值，φ_th為設(shè)定的航向角閾值。

4.根據(jù)權(quán)利要求1所述的基于加性四元數(shù)的大方位失準(zhǔn)角線性對準(zhǔn)方法，其特征在于：所述步驟(3)中的小角度為小于等于5°。