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

通過對陀螺和加速度計的輸出值進行處理，來估計計算地裏座標系和真實導航座標系之間的小失準角，從而校正粗對準獲得的捷聯矩陣的過程便是精對準。 

精對準過程可以以開環的方式進行，即在估計、 的過程中，並不對失準角進行即時的校正，在對準結束後，一次性修正失準角，例如上節提到的一次修正對陣。

從誤差方程來分析系統的初始精對準，粗對準結束後誤差角均爲小角度。靜基座對準時，水平誤差角比方位誤差角要小，故可以略去交叉耦合項，,，同時載體的位置是已知的，故可略去，系統的誤差方程爲：

                                                                                     （1）

初始對準過程時間通常都比較短，因此慣性器件的模型可以近似的表示爲常值零偏和白噪聲相結合的輸出值，那麼慣性器件的模型爲： 

                                                                                       （2）

在誤差方程的基礎上，把陀螺儀的常值漂移和加速度計的常值零偏作爲系統的狀態量，因此狀態變量爲:

                                                                  （3）

爲加速度計在載體座標系下xy方向的隨機常值偏差，爲陀螺儀在載體座標系下xyz方向的隨機常值漂移，爲東、北向速度誤差，爲方位失準角。

狀態方程如下：

                                                                                                        （4）

其中：

（5）

 ，是由加速度計零偏和陀螺儀漂移引起的模型誤差。

                                                                                 （6）

觀測方程

                                                       （7）

後者爲觀測噪聲，服從高斯分佈N(0,R)。

將（4）與（7）進行離散化，一般用A作爲系統方程，用單位矩陣代替G上半部分，最終進行卡爾曼濾波。

%精對準（卡爾曼濾波）
%誤差參數
V_delta = 0.01;
W_epsilon = degree2radian(0.5)/3600;
W_d = degree2radian(0.5)/3600;
F_delta = 1e-5*g_unit;
F_d = 1e-5*g_unit;
PHI = degree2radian(1);

%數值初始化
Xk = [0 0 0 0 0 0 0 0 0 0]';
PHIx_0 = Xk(3);
f_PHIx_0 = PHIx_0;
Pk = diag([V_delta V_delta PHI PHI PHI F_delta F_delta W_epsilon W_epsilon W_epsilon].^2);
Q = diag([F_d F_d W_d W_d W_d 0 0 0 0 0].^2);
R = diag([V_delta V_delta].^2);

%系統矩陣
F = zeros(10,10);
F(1,2) = 2*Weie(3)*sin(L);
F(2,1) = -2*Weie(3)*sin(L);
F(3,4) = Weie(3)*sin(L);
F(3,5) = -Weie(3)*cos(L);
F(4,3) = -Weie(3)*sin(L);
F(5,3) = Weie(3)*cos(L);
F(1,4) = -g;
F(2,3) = g;
%F(3,2) = -1/RM;
%F(4,1) = 1/RN;
%F(5,1) = tan(L)/RN;
F(3,2) = -1/(RM + H);
F(4,1) = 1/(RN + H);
F(5,1) = tan(L)/(RN + H);
F(1:2,6:7) = Tnb(1:2,1:2);
F(3:5,8:10) = Tnb;

G = eye(10);

%量測陣
Hk = zeros(2,10);
Hk(1,1) = 1;
Hk(2,2) = 1;

%連續時間系統離散化
PHIk_k_1 = eye(10) + F.*deltaT + F^2.*(deltaT^2/2);%一步轉移陣
GAMMAk_1 = deltaT.*(eye(10) + F.*(deltaT/2) + F^2.*(deltaT^2/6))*G;%系統噪聲驅動陣

%計算量測量初值
Vn = [0;0;0];%Zk
Fb = -Fb;
Wnen = W_N_E_N(Vn(1),Vn(2),L,H);
d_Vn_0 = d_V_N(Tnb,Fb,Wnen,Cne,Vn,g);

%err = 1;%epsilon = 1e-20;%phi_0 = [pi;pi];
prog = 1;
PHI_res = [];
%遞推濾波
%while((prog <= 55000)&&(err > epsilon))
while((prog <= 55000))
    %讀IMU數據（計算量測量）
    imu = fscanf(fidIn,'%e',[7,1]);
    Fb = [imu(2);imu(3);imu(4)];
    
    d_Vn = d_V_N(Tnb,Fb,Wnen,Cne,Vn,g);
    
    Vn(1) = R_K_2(deltaT,Vn(1),d_Vn_0(1),d_Vn(1));
    Vn(2) = R_K_2(deltaT,Vn(2),d_Vn_0(2),d_Vn(2));
    
    d_Vn_0 = d_Vn;
    
    Wnen = W_N_E_N(Vn(1),Vn(2),L,H);
    
    %離散型卡爾曼濾波基本方程
    Pk_k_1 = PHIk_k_1*Pk*PHIk_k_1' + GAMMAk_1*Q*GAMMAk_1';
    Kk = Pk_k_1*Hk'/(Hk*Pk_k_1*Hk' + R);
    %Pk = inv(inv(Pk_k_1) + Hk'/R*Hk);
    Pk = (eye(10) - Kk*Hk)*Pk_k_1*(eye(10) - Kk*Hk)' + Kk*R*Kk';
    Xk_k_1 = PHIk_k_1*Xk;
    Xk = Xk_k_1 + Kk*(Vn(1:2,:) - Hk*Xk_k_1);
    
    %低通濾波
    f_PHIx = Filter_PHIx(PHIx_0,Xk(3),f_PHIx_0);
    %d_phi_x = F(3,:)*Xk;
    d_phi_x = (f_PHIx - f_PHIx_0)./deltaT;
    PHIx_0 = Xk(3);
    f_PHIx_0 = f_PHIx;
    
    %計算姿態角
    phi_z = (-d_phi_x + Xk(4)*Weie(3)*sin(L) - Xk(2)/RM)/(Weie(3)*cos(L));
    phi = [Xk(3:4);phi_z];
    Tnb_k = (eye(3,3) + OmegaMatrix(phi))*Tnb;
    [psi,theta,gamma] = AnttitudeAngle_Tnb(Tnb_k);
    anttitude_res = [anttitude_res;[psi theta gamma]];
    
    PHI_res = [PHI_res [Xk(3:5);phi_z]];
    
    %phi = phi(1:2);
    %err = max([RelativeError(phi_0(1),phi(1)) RelativeError(phi_0(2),phi(2))]);
    %phi_0 = phi;
    disp(prog);
    prog = prog + 1;
end