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