SINS/GNSS組合導航：捷聯慣導靜基座下初始對準 （二）一次修正粗對準

SINS初始對準要測定系統的姿態變換矩陣，分爲粗對準和精對準兩個部分，粗短準階段利用重力和地球自轉量粗略計算姿態矩陣；在精對準階段，不僅依靠前面的姿態矩陣測量值及重力和地球自轉量，還要依據慣性器件的輸出值和觀測量，用合適的濾波方法，對系統的誤差量進行估計，修正姿態矩陣，計算出精確的姿態變換矩陣。 

靜態基座下一次修正粗對準

粗對準參考文章：https://blog.csdn.net/LittleEmperor/article/details/105452930。實際上粗對準得到了b繫到計算機地理座標系p系之間的轉換矩陣，計算機地理座標系是導航計算機求解得到的，這與真實的地理座標系e是有差別的。一次修正粗對準就是減小p系與e系之間的誤差。

設兩個系之間的夾角爲。反對稱矩陣：

                                                                                                    （1）

e繫到p系的變換矩陣如下：

                                                                                  （2）

p繫到e系的變換矩陣如下：

                                                                                  （3）

粗對準導航計算機得到了b繫到p系的姿態矩陣，這個精度很低，不能當做b繫到e系的姿態矩陣，可以得到：

                                                                                                   （4）

準確的姿態矩陣如下：

                                                                                      （5）

要想得到精確的姿態矩陣，就需要得到精確的及失準角。

失準角計算公式如下：失準角，滿足如下：

                                                                       （6）

最終根據公式（3）計算出較精確的姿態矩陣。

代碼如下：

%INS讀初值
dataPath = '../Data/';
fidOut = fopen([dataPath,'INS.txt'],'r');
initOut = fscanf(fidOut,'%e',[13,1]);
lambda = degree2radian(initOut(2));
L = degree2radian(initOut(3));
H = initOut(4);
g = G_LH(L,H);
[RM,RN] = R_M_N(L);
Cne = C_N_E(lambda,L);

%IMU讀數據用於一次修正粗對準（對數據做平均消減隨機誤差的影響）
Wbib = [];
Fb = [];
for i = 1:2500
    imu = fscanf(fidIn,'%e',[7,1]);
    Wbib = [Wbib [imu(5);imu(6);imu(7)]];
    Fb = [Fb [imu(2);imu(3);imu(4)]];
end
Wbib = sum(Wbib,2)./size(Wbib,2);
Fb = sum(Fb,2)./size(Fb,2);

Fb = -Fb;

%一次修正粗對準
Fn = Tnb*Fb;
Wnib = Tnb*Wbib;
%phi = [Fn(2)/g;-Fn(1)/g;Wnib(1)/(Weie(3)*cos(L)) - Fn(1)*tan(L)/g];
phi = [-Fn(2)/g;Fn(1)/g;Wnib(1)/(Weie(3)*cos(L)) + Fn(1)*tan(L)/g];
Tnb = (eye(3,3) + OmegaMatrix(phi))*Tnb;

%結果輸出
[psi,theta,gamma] = AnttitudeAngle_Tnb(Tnb);
disp('一次修正粗對準結果：');
disp(['ψ：',num2str(radian2degree(psi)),'°']);
disp(['θ：',num2str(radian2degree(theta)),'°']);
disp(['γ：',num2str(radian2degree(gamma)),'°']);