PSINS初始对准方法

初始对准方法

初始对准方法较多，适用环境不同：
alignsb：解析式对准，适用于静基座或微晃动环境；
aligni0：惯性凝固系初始对准（间接对准），抗角晃动干扰能力强(同上)；
aligncmps：罗经法精对准(基本不用了)；
alignfn/vn：以比力/速度误差为观测量的Kalman滤波精对准；
aligni0vn：基于数据复用技术，惯性系粗+速度量测Kalman滤波精对准(地球人都知道的最好的初始对准方法)；
还有一些示例函数：
test_align_ekf：大方位失准角EKF滤波初始对准仿真(当方位角很大，两个水平角很小时)；
test_align_ukf：大失准角EKF滤波初始对准(当三个角都很大时)；
test_align_two_position：双位置对准(在0位置惯导停一段时间，旋转180度(增加或减少)，在对面的角位置又停了相同时间)；
test_align_rotation：单轴旋转对准；
test_align_transfer：速度+姿态匹配传递对准；
test_align_methods_compare/lgimu几种对准方法比较仿真/实测数据。

惯性系粗对准+数据存储+Kalman滤波精对准例子

aligni0vn函数代码如下：

function att1 = aligni0vn(imu, pos, t1)
% SINS initial align based on inertial-frame & vn-meas method.
%
% Prototype: att0 = aligni0(imu, pos, t1)
% Inputs: imu - IMU data
%         pos - position
%         t1 - inertial-frame align time
% Output: att1 - attitude align result
%
% Example:
%     glvs;
%     [imu, avp0, ts] = imufile('lasergyro.imu');
%     att = aligni0vn(imu(1:300/ts,:), avp0(7:9)', 180);
%
% See also  aligni0, alignvn.

% Copyright(c) 2009-2020, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/11/2020
global glv
    ts = imu(2,end)-imu(1,end);
    if nargin<3, t1 = 30; end
    if(length(pos)>4) pos=pos(4:6); end
    imu1 = imu(1:fix(min(imu(end,end)-imu(1,end),t1)/ts),:);
    [att0, res0] = aligni0(imu1, pos, ts);  % coarse align
    [att0, iatt] = attrvs(imu1, att0, pos);  % reverse attitude update
	phi = [1.1; 1.1; 10]*glv.deg;
	imuerr = imuerrset(0.002, 20, 0.001, 10);
	wvn = [0.01; 0.01; 0.01]*10;
	[att1, attk] = alignvn(imu, a2qua(att0), pos, phi, imuerr, wvn);  % fine align
    insplot(attk,'a');
    subplot(211), plot(res0.attk(:,end), res0.attk(:,1:2)/glv.deg, 'm');
    subplot(212), plot(res0.attk(:,end), res0.attk(:,3)/glv.deg, 'm');
    subplot(212), plot(iatt(:,end), iatt(:,3)/glv.deg, ':r', 'linewidth',2); legend('KF fine yaw', 'i0 coarse yaw', 'reverse yaw');
    for k=1:2  % reverse & fine iteration
        [att0, iatt] = attrvs(imu, attk(end,1:3)', pos);
        subplot(212), plot(iatt(:,end), iatt(:,3)/glv.deg, ':r', 'linewidth',2);
        phi = [0.1; 0.1; 1]/k*glv.deg;
        [att1, attk] = alignvn(imu, a2qua(att0), pos, phi, imuerr, wvn);  close(clf);
        subplot(211), plot(attk(:,end), attk(:,1:2)/glv.deg, 'linewidth',k+1);
        subplot(212), plot(attk(:,end), attk(:,3)/glv.deg, 'linewidth',k+1);
    end

示例结果如下：

上图为姿态/方位角收敛过程


上述对准表示对准到180s，惯性系粗对准可以得到初始时刻姿态，然后后面利用卡尔曼滤波进行对准
输入的参数为：
imu - IMU data，IMU数据
pos - position，初始位置信息
t1 - inertial-frame align time，惯性系粗对准时间

大方位失准角线性Kalman滤波与EKF滤波初始对准比较

打开例子所在文件demos\test_align_ekf.m，并运行：

上图为状态估计误差比较

大失准角UKF滤波初始对准

打开例子所在文件demos\test_align_ukf.m，并运行：

上图为状态估计及其均方误差

经验

1)通常水平失准角估计速度比较快，而方位失准角慢，采用遗忘滤波算法有利于避免状态估计方差阵和增益矩阵的退化，提高方位收敛速度;
2)滤波过程中当失准角降低至比较小时，从大失准角UKF滤波方式转到小失准角Kalman滤波方式能够降低计算量，并且不损失对准精度;
3)对于一般非线性系统，目前还很难从理论上证明UKF滤波的有效性和收敛性，而主要依靠仿真和试验验证。如果在大失准角条件下使用UKF 滤波的主要目的在于迅速辨识粗略失准角的话，在KF滤波模型中就可以不将惯性传感器误差纳入滤波模型，降低维数不仅能能够减小计算量，还有利于减小UKF 滤波发散的机会。