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

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

靜態基座下解析粗對準

粗對準的目的是快速的得到一個粗略的姿態矩陣，即載體系b到導航系e的姿態矩陣，爲精對準做準備，因此縮短對準時間是粗對準的重要目的。粗對準時，忽略干擾信息的影響，認爲載體處於理想的靜止狀態，把地球自轉角速度和重力矢量信息作爲粗對準時刻陀螺儀和加速度計的信息，根 據公式推導計算出姿態矩陣。

靜基座狀態下，經度緯度確定，由重力計算公式可以精確的求出當地的重力加速度，同時可以求出精確的地球自轉角速率，他們在地理座標系的投影可以表示爲：

                                                              （1）

構造新的向量，得到，並且b系表示如下：

                                                                                                                        （2）

是正交矩陣，滿足，將（7）轉置可以得到：

                                                                                                              （3）

寫成矩陣的形式：

                                                                                                   （4）

取逆分離得到：

                                                                                               （5）

將（6）取逆矩陣，並與（7）代入（10）即可得到：

                             （7）

是陀螺儀的輸出，是加速度計的輸出，根據這兩個信息叉乘得到。爲地球重力加速度，爲地球自轉角速度的。

通過上述過程，粗對準實現了由加速度計和陀螺輸出到姿態矩陣的轉換，得到粗略的對準矩陣，代碼如下：

%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讀數據用於粗對準（對數據做平均消減隨機誤差的影響）
fidIn = fopen([dataPath,'IMU.txt'],'r');
Wbib = [];
Fb = [];
for i = 1:2500
    imu = fscanf(fidIn,'%e',[7,1]);
    Wbib = [Wbib [imu(5);imu(6);imu(7)]]; %#ok<*AGROW>
    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;

%粗對準
Vn_T_inv = [0 0 1/(g*Weie(3)*cos(L));tan(L)/g 1/(Weie(3)*cos(L)) 0;-1/g 0 0];
Vb_T = [Fb(1) Fb(2) Fb(3);Wbib(1) Wbib(2) Wbib(3);...
    Fb(2)*Wbib(3) - Fb(3)*Wbib(2) Fb(3)*Wbib(1) - Fb(1)*Wbib(3) Fb(1)*Wbib(2) - Fb(2)*Wbib(1)];
Tnb = Vn_T_inv*Vb_T;

%Tnb = orth(Tnb);%Tnb = Normalization(Tnb,1);%Tnb = Orthogonalization_Schmidt(Tnb);
[psi,theta,gamma] = AnttitudeAngle_Tnb(Tnb);
Tnb = T_N_B(psi,theta,gamma);

%結果輸出
disp('粗對準結果：');
disp(['ψ：',num2str(radian2degree(psi)),'°']);
disp(['θ：',num2str(radian2degree(theta)),'°']);
disp(['γ：',num2str(radian2degree(gamma)),'°']);