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)),'°']);