Psins代码解析之静基座仿真（test_SINS_static.m）&傅科、修拉周期&水平通道误差传播（test_SINS_static_verify.m）

静基座条件下：速度矢量为0；地理位置精确已知；

一、静基座下，陀螺仪和加速度计数据仿真： imustatic.m

重力矢量和地球自转角速度在地理坐标系下：

上式中，Wnen为0，Wnnb为0，即静基座下，陀螺仪的仿真变为：Cbn*eth.wnie

上式中，速度为0、速度微分为0，即静基座下，加速度计的仿真为：-Cbn*eth.gn

imu = [Cbn*eth.wnie; -Cbn*eth.gn]';

添加IMU误差：imuerr = imuerrset(0.01, 10, 0.001, 0.2); %添加IMU误差

 代码实现为：

    if ~exist('imuerr', 'var'), imuerr = imuerrset(0,0,0,0); end
    Cbn = a2mat(avp0(1:3))';
    eth = earth(avp0(7:9), avp0(4:6));
    imu = [Cbn*eth.wnie; -Cbn*eth.gn]';
    if nargin<2,  ts = 1; T = ts;  end
    if nargin<3,  T = ts;  end
    len = round(T/ts);
    imu = repmat(imu*ts, len, 1);
    if nargin==4
        imu = imuadderr(imu, imuerr, ts);
    end
    imu(:,7) = (1:len)'*ts;

添加初始对准误差：davp0 = avpseterr([-10;10;3], [0.01;0.01;0.01], [10;10;10]);

二、纯捷联惯导算法仿真

进行捷联惯导解算，其中部分子函数解析详见：Psins代码解析之常用的子函数

avp = inspure(imu, avp00, avp0(9));  % pure inertial navigation

avperr = avpcmp(avp, avp0);

inserrplot(avperr);

纯惯性导航中，高度通道发散（涉及到天向速度和天向高度）；因此，在程序中，使用高度阻尼进行高度限制；

1、Pure SINS altitude damping using Kalman filter.使用卡尔曼滤波器的纯SINS高度阻尼

首先是设置kalman滤波器的初始参数：

状态转移矩阵F、H矩阵、Q矩阵、状态X阵、R矩阵、状态方差矩阵等

2、设置全局变量 igaln ，此处只是初始化得到全局变量：

    if vp_fix=='n';
        alt = altfilt(1000, 1*glv.ugpsHz, 10.0, nts);  % altitude damping setting
        imugpssyn(imu(:,7), href(:,2)); %
        dbU = href; dbU(:,1) = 0;
    end

3、在捷联惯导解算循环算法中：用imugpssyn.m函数， 目的是找到IMU和GPS同步的时间，以便为后面的kalman滤波做准备

具体实现源代码：

        elseif vp_fix=='n',
            alt = altfilt(alt);
            [khref, dt] = imugpssyn(k, k1, 'F');
            if khref>0
                dh = ins.pos(3)-ins.vn(3)*dt - href(khref,1); %惯导解算位置 - 参考位置
                alt = altfilt(alt, dh);
                ins.pos(3) = ins.pos(3) - alt.xk(3);  % vertical vn&pos feedback
                ins.vn(3) = ins.vn(3) - alt.xk(2);    alt.xk(2:3) = 0;
                dbU(khref,1) = alt.xk(1); % just for plot debug
            end

其中，每次循环解算时，都要进行高度滤波器的时间更新，然后时间判断，如果imu和gps时间同步了，则利用dh进行滤波器的量测更新，并进行反馈；同时将误差状态x中的速度误差、高度误差置0，加速度零偏保留；

4、结果输出：

（1）添加IMU误差和初始姿态、速度、位置误差的计算结果：

最终解算完成后，利用kalman估计的天向加速度计零偏为;

纯惯导解算【avp】为：

纯惯导惯导解算和参考真值误差为：

（2）无IMU误差和初始姿态、速度、位置误差的计算结果：

如果将IMU误差置为0，即无误差的陀螺仪和加速度计数据；同时初始姿态、速度、位置也是无误差的精确数据，则计算结果如下：

avp0 = avpset([0;0;0], [0;0;0], glv.pos0);
% imuerr = imuerrset(0.01, 10, 0.001, 0.2); %添加IMU误差
imuerr = imuerrset(0., 0, 0.00, 0.); %添加IMU误差
imu = imustatic(avp0, ts, T, imuerr);   % SIMU simulation
% davp0 = avpseterr([-10;10;3], [0.01;0.01;0.01], [10;10;10]);
davp0 = avpseterr([0;0;0], [0.0;0.0;0.0], [0;0;0]);
avp00 = avpadderr(avp0, davp0);

最终解算完成后，利用kalman估计的天向加速度计零偏为; 

纯惯导解算【avp】为：

纯惯导惯导解算和参考真值误差为：

5、通过上面可以看出：

其中静基座条件下，通过分析，可以得到三种角频率：休拉(Schuler)角频率、傅科角频率、

从上面可以看出：由于系统时无阻尼振荡系统；且仿真时间为24小时，纬度为：

glv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380];

此时的傅科周期为：T=(2*pi)/(glv.wie*glv.pos0(1))/3600=40.0438hour

仿真40小时的数据，IMU数据添加初始零偏误差：

得到捷联惯导解算的姿态、速度、位置为：

avp误差为：

三、捷联惯导水平通道解算误差和理论误差：

静基座下，地理位置已知，速度为0，姿态角为0；

注意：解算过程中，强制将天向速度和天向高度置初值；

glvs
T = 40*60*60;  % total simulation time length
[nn, ts, nts] = nnts(1, 1);
avp0 = avpset([0;0;0], [0;0;0], [34;110;380]);  eth = earth(avp0(7:9));
imuerr = imuerrset([0;0;0.], [0;0;0], 0.0, 0.0);
imu = imustatic(avp0, ts, T, imuerr);   % SIMU simulation
% davp0 = avpseterr([0;0;0], [0.0; 10.0;0.0], [0;0;0]);
davp0 = avpseterr([0;0;0], [0.0; 0.0;0.0], [0;0;0]);
avp00 = avpadderr(avp0, davp0);
ins = insinit(avp00, ts);
len = length(imu);    avp = zeros(fix(len/nn), 10);
ki = timebar(nn, len, 'Pure inertial navigation processing.');
for k=1:nn:len-nn+1
	k1 = k+nn-1;
	wvm = imu(k:k1, 1:6);  t = imu(k1,7);
	ins = insupdate(ins, wvm);  ins.vn(3) = 0;  ins.pos(3) = avp0(9); %天向速度为0；高度为初值，保持不变
	avp(ki,:) = [ins.avp; t]';
	ki = timebar;
end
avperr = avpcmp(avp, avp0);
inserrplot(avperr);

状态为：姿态角误差、水平速度误差(东、北向)、位置误差(纬、经度)

%% 前面是惯导解算误差，后面是理论误差
R = sqrt(eth.RNh*eth.RMh); L = avp0(7); tL = tan(L); eL = sec(L);
wN = eth.wnie(2); wU = eth.wnie(3); g = -eth.gn(3);
qnb = a2qua(avp0(1:3)); en = qmulv(qnb,imuerr.eb); dn = qmulv(qnb,imuerr.db);
X0 = [davp0([1:5,7:8])]; U = [-en; dn(1:2); 0; 0];
F = [ 0   wU -wN  0    -1/R   0   0
     -wU  0   0   1/R   0    -wU  0  
      wN  0   0   tL/R  0     wN  0  
      0  -g   0   0     2*wU  0   0  
      g   0   0  -2*wU  0     0   0  
      0   0   0   0     1/R   0   0  
      0   0   0   eL/R  0     0   0 ];
[Fk, Bk] = c2d(F, eye(size(F)), ts); Uk = Bk*U;  % 离散化
Xk = X0; XX = zeros(len,8);
for k=1:len
    Xk = Fk*Xk+Uk;
    XX(k,:) = [Xk; k*ts]';
end
figure
subplot(221), plot(XX(:,end), XX(:,1:2)/glv.sec, 'm');
subplot(222), plot(XX(:,end), XX(:,3)/glv.min, 'm');
subplot(223), plot(XX(:,end), XX(:,4:5), 'm');
subplot(224), plot(XX(:,end), XX(:,6:7)*glv.Re, 'm');

运行结果为：

（1）.因为IMU数据没有添加任何误差，同时，初始姿态、速度、位置没有任何误差；完全干净的数据，其水平通道仿真40个小时得到的结果如下：

可以看到，其出现周期性误差，其原因归于：

• 数值解算精度，并且符合休拉周期和傅科周期；
• 捷联惯导递推过程中，涉及到临时变量（地球参数）的更新，速度、姿态、位置参数的更新，其算法存在不饿完全准确;

（2）.基于误差状态的微分方程进行误差状态计算，得到结果如下：

理论计算完全无误差！