Psins代码解析之kalman松组合导航融合算法 test_SINS_GPS_153.m&test_SINS_GPS_186.m&test_SINS_GPS_193.m

左手的ㄟ右手 2023-07-23 06:59 59阅读 0赞

框架：

*  设置松组合导航算法中状态量、观测量数目；比如：psinstypedef(153)，状态误差量为15维，量测量为3维；
 *  对仿真生成的飞行轨迹(test\_SINS\_trj.m)，设置IMU误差；并同时对初始AVP参数设置误差；
 *  对kalman滤波器的参数进行初始化：

% KF filter
    rk = posseterr([1;1;3]); %位置误差
    kf = kfinit(ins, davp0, imuerr, rk);

### **1、kalman滤波器参数初始化：** ###

case psinsdef.kfinit153,
            psinsdef.kffk = 15;  psinsdef.kfhk = 153;  psinsdef.kfplot = 15;
            [davp, imuerr, rk] = setvals(varargin);
            kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2; %注意此处是Qt
            kf.Rk = diag(rk)^2;
            kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
            kf.Hk = kfhk(0);
    kf = kfinit0(kf, nts);

最终得到离散化的kf参数，如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

“1”的计算：**kf.Qt = diag(\[imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)\])^2;**

“2”的计算：**kf.Qk = kf.Qt\*kf.nts; end %离散化的Q**

“3”的计算：**kf.xtau = ones(size(kf.xk))\*eps; 示例代码中给出的是一个matlab能够分辨的最小值；**

“4”的计算：**kf.xfb(idx) = kf.xfb(idx) + xfb\_tmp(idx);** % record total feedback，对进行误差反馈的状态量，每次有反馈都进行反馈的累加

“5”的计算：xtau = kf.xtau;  
xtau(kf.xtau<kf.T\_fb) = **kf.T\_fb**; kf.coef\_fb = kf.T\_fb./xtau; %2015-2-22 反馈系数

其中 kf.T\_fb = 1; end %时间标签，**即kalman数据融合时间**，此处等于1秒

### **2、捷联惯导解算：纯惯导解算和kalman时间更新、量测更新；** ###

**kalman滤波流程图：**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

**（1）时间更新：**

采用双子样进行惯导解算，并且对误差状态进行、误差状态方差进行预测更新；

wvm = imu(k:k1,1:6);  t = imu(k1,end);
        if k>200*100
            wvm(:,6) = wvm(:,6)+50*glv.ug*ts;
        end
        ins = insupdate(ins, wvm);
        kf.Phikk_1 = kffk(ins); %discrete-time transition matrix 得到离散的状态转移矩阵
        kf = kfupdate(kf); %执行时间更新

状态转移矩阵的求取为：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2][]

离散化：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3][]

执行状态更新：kf.Gammak=1;

![20200407160047635.png][]

**（2）量测更新：**

主要进行：量测一步预测、量测一步预测均方误差阵、状态一步预测与量测一步预测之间的协均方误差阵、增益矩阵K计算、状态最优估计、状态估计均方误差阵；

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4][]

代码如下：

if mod(t,1)==0 %整秒进行kalman滤波的量测更新；
            posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noise
            kf = kfupdate(kf, ins.pos-posGPS, 'M');
            [kf, ins] = kffeedback(kf, ins, 1, 'vp');
            avp(ki,:) = [ins.avp', t];
            xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
        end

量测更新：

if TimeMeasBoth=='M'        % Meas Updating
                kf.xkk_1 = kf.xk;    
                kf.Pxkk_1 = kf.Pxk; 
            elseif TimeMeasBoth=='B'    % Time & Meas Updating
                kf.xkk_1 = kf.Phikk_1*kf.xk;    
                kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
            else
                error('TimeMeasBoth input error!');
            end
            kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';
            kf.Py0 = kf.Hk*kf.Pxykk_1;
            kf.ykk_1 = kf.Hk*kf.xkk_1;
            kf.rk = yk-kf.ykk_1;
            kf.Pykk_1 = kf.Py0 + kf.Rk;
            kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); % kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1;
            kf.xk = kf.xkk_1 + kf.Kk*kf.rk;
            kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';
            kf.Pxk = (kf.Pxk+kf.Pxk')*(kf.fading/2); % symmetrization & forgetting factor 'fading'

误差状态反馈、校正导航系统：**\[kf, ins\] = kffeedback(kf, ins, 1, 'vp');**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5][]

**注意，因为系统是基于误差状态量的kalman组合导航系统，因此，当系统估计除了误差状态，毕竟对相应的状态量做了补偿，则需要在误差状态量中予以扣除；**

![20200407162011793.png][]

### **3、误差显示：** ###

**（1）kalman组合导航结果输出：【avp】**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6][]

**（2）kalman组合导航结果与参考真值做差输出：****avperr = avpcmp(avp, trj.avp)**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7][]

仿真轨迹如下：参考链接[Psins代码解析一之全局变量&轨迹仿真（test\_SINS\_trj.m）&惯性解算（test\_SINS.m）][Psins_test_SINS_trj.m_test_SINS.m]

**（3）kalman组合导航中，最优估计误差状态量输出：【**kf.xk**】**

inserrplot(xkpk(:,[1:psinsdef.kfplot,end]));
    
            case 'avped'
                myfigure;
                subplot(321), plot(t, err(:,1:2)/glv.sec); xygo('phiEN');
                subplot(322), plot(t, err(:,3)/glv.min); xygo('phiU');
                subplot(323), plot(t, err(:,4:6)); xygo('dV');
                subplot(324), plot(t, [err(:,7:8)*glv.Re,err(:,9)]); xygo('dP');
                subplot(325), plot(t, err(:,10:12)/glv.dph); xygo('eb');
                subplot(326), plot(t, err(:,13:15)/glv.ug); xygo('db');

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8][]

**（4）**粉红色线条为 **avperr = avpcmp(avp, trj.avp)；即avperr与**xkpk整合在一张图上；

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9][]

**（5）**diag(kf.Pxk) ，sqrt(误差状态估计均方误差阵图)：对角线元素：

inserrplot(\[sqrt(xkpk(:,psinsdef.kfplot+1:end-1)),xkpk(:,end)\]);

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 10][]

### **4、杆臂效应和时间对齐误差；** ###

**（1）杆臂效应，参数估计：**

在组合导航中，一般以IMU器件所在的位置为姿态、速度、位置的基准；由于GNSS接收机天线的安装与IMU器件有偏差，存在杆臂效应，如下图所示：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 11][]

杆臂：由IMU指向GNSS天线；基于此，推导出INS和GNSS速度、位置关系；

![20200408115857694.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 12][]

其中，Mpv为：

![20200408120101322.png][]

状态误差量为：18-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3)

量测矩阵为：Hk = \[zeros(6,3), eye(6), zeros(6,6), \[-ins.CW;-ins.MpvCnb\]\];

**（2）时间不同步，参数估计：**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 13][]

以图示为例：卫星信号和惯导信号时间不同步误差为δt，如果惯导在两个采样时刻间的速度率变化为an，则有如下计算公式：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 14][]

即，速度、位置不同步误差为：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 15][]

状态误差量为：19-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)

量测矩阵为：Hk = \[zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, ins.Mpvvn\]; %位置误差参与量测更新

参考：

状态空间模型为：19-state included: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 16][]

**（3）以test\_SINS\_GPS\_193.m为例：**

> 误差状态量为：phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)；
> 
> 量测状态为：Δpos

需要注意的几个点：

*  利用trj生成GPS轨迹时，需要添加lever、dT误差(lever = \[1; 2; 3\]、dT = 0.1)；在程序中，lever是以IMU中心为基准; 而dT为IMU和GPS数据之间的时间不对齐误差， 此处指gps时间超前IMU时间，如下图所示：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 17][]

*  量测矩阵H的建立：

Hk = \[zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb, -ins.Mpvvn\];

其中： ”-ins.MpvCnb, -ins.Mpvvn“ 在每次进行量测更新时，都需要更新；其在ins = inslever(ins)中实现。

*  量测更新中，Zk的编排：

kf = kfupdate(kf, ins.posL-ins.Mpvvn\*dt-posGPS, 'M')

画图展示：程序中采用“avp”反馈；预设lever=\[1;2;3\] 、dT = 0.1;

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 18][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20230528/e74c7eb084a44233b076cd0edbc0dbed.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: /images/20230528/f6e79b42b44c44f08fbd1d546ea87b4c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2]: /images/20230528/009aadfb31b64e229ff3440b9b9331ff.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3]: /images/20230528/86ff4073cb574099b3b79781dc57c5e0.png
[20200407160047635.png]: /images/20230528/652518f84d74404ea8050b8cfee16423.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4]: /images/20230528/78b6189bf3274f6fae9a972485f78f8f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5]: /images/20230528/94b662d3f7c84e27855b20acf7b4e729.png
[20200407162011793.png]: /images/20230528/f9974661289a4bdfb2b05f00c07bffcb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6]: /images/20230528/1df97026fb8646dcaf88f0decce8618b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7]: /images/20230528/d5c0189d968749d590d62c0d8f61e30a.png
[Psins_test_SINS_trj.m_test_SINS.m]: https://blog.csdn.net/wuwuku123/article/details/105311738
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8]: /images/20230528/d712467d2b884f83bd54efe422d558f5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9]: /images/20230528/900048cfd02143beab4bee77d03b8194.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 10]: /images/20230528/d814afa5d2ed40cc9da9ccf22fb8784e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 11]: /images/20230528/1aa4044d66ee401da34eb5b30a644e33.png
[20200408115857694.png]: /images/20230528/86731ac305c34deeb440bc19be2e9a8b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 12]: /images/20230528/fb7ff58c6b6747b0a475c928f23e431b.png
[20200408120101322.png]: /images/20230528/eed85019354d462aadf6472b69e3dd7d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 13]: /images/20230528/4dd445f6ab564c448a96e41ce6ccfcbf.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 14]: /images/20230528/3ad69b3077804360b8a06dde7459e7f2.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 15]: /images/20230528/8c4d2a000f374992b8df64d994343afd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 16]: /images/20230528/ee809e0e0ae549a79222baa824b1a013.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 17]: /images/20230528/b4373be835f54436bc5cb62c1e916687.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 18]: /images/20230528/ef8b6db069774f14ac7c6172ab13c8ac.png