通过对陀螺和加速度计的输出值进行处理,来估计计算地里坐标系和真实导航坐标系之间的小失准角,从而校正粗对准获得的捷联矩阵的过程便是精对准。
精对准过程可以以开环的方式进行,即在估计、 的过程中,并不对失准角进行即时的校正,在对准结束后,一次性修正失准角,例如上节提到的一次修正对阵。
从误差方程来分析系统的初始精对准,粗对准结束后误差角均为小角度。静基座对准时,水平误差角比方位误差角要小,故可以略去交叉耦合项,,,同时载体的位置是已知的,故可略去,系统的误差方程为:
(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