Psins之 test_SINS_GPS_153代码解析

先大致理一下流程：

第一步：

用增量得到phim和dvbm，并减去加速度计和陀螺引起的误差

第二步：

计算得出ins.web和ins.wnb

第三步：

1.计算导航坐标系下的加速度，并更新vn1=ins.vn+ins.an*nts

2.进行位置更新，对用中值法对速度进行计算，得到位置变化量

3.姿态更新

第四步：

进行卡尔曼滤波，分为两种情况，第一种若没有或者没有合适的量测情况下，只进行滤波一步预测与预测误差协方差阵的计算：

若适合进行与观测值融合，那么进入剩下的三个公式。

 大致流程如上，但是最后一个第5个公式的代码不太明白：

 对于估计误差协方差阵，有以下几种表示：

-----------------------------------------------------------后续增加----------------------------------------------------------

当滤波完成后，后面还有一个限制Pk的操作：

Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax；使Pk在一定的范围内。

滤波完成后，需要对进行滤波反馈：

进入： [kf, ins] = kffeedback(kf, ins, 1, 'avp');

我们直接看循环，循环前只做了把kf.xk的值传递给xfb_tmp;

 循环内做了用惯导计算出的值减去卡尔曼滤波估计出的姿态、速度、位置误差。

第一行代码：每次减完都需要把当前时刻的估计误差给扣除，也就是最后把姿态、速度、位置误差全部清零。

第二行：把每次的误差大小都叠加起来。

最后重新计算反馈后的姿态、速度、位置，把这个值作为惯导的值。退出滤波反馈函数。

剩下的就是记录并且画图了，不是重点。

等效旋转矢量基于泰勒级数展开的多字样算法推导大致如下：

首先假设角速度的形式

然后推导出连续形式下的角增量

最后消去a、b、T得到等效旋转矢量的表达。 

一般不优先考虑此方法，解释如下：

---

下面简单推导基于圆锥误差补偿的多字样算法，具体推导见严老师书2.6节：

 假设圆锥运动条件下的陀螺仪角速度：

 Ω为振动频率

经过一番推导得在tm-1和tm之间的等效旋转矢量近似表达式：

 然后对角速度积分得角增量：

 

 我们都是用角增量来代替旋转矢量，但是我们发现上述推导的角增量和旋转矢量在z轴上不同，因此，我们采用旋转矢量来进行姿态更新时会产生误差，且会随时间不断积累。这个误差表达如：

 后续推导过于复杂了，直接给出结论：

 补偿形式如下：

本文选择nn=1；也就是单子样。

代码主体为两部分：第一部分为惯导更新，第二步为卡尔曼滤波。

首先看insupdate

 cnscl函数为圆锥运动和划桨补偿：

 当为单子样时，应该是没有圆锥运动补偿的，dphim一直为[0,0,0];

下面进入划桨补偿:

根据书82页的公式：

 代码应该没有考虑等号右边最后一项有害加速度的计算，

对于等号右边第二项为比力速度增量也是比较复杂的，代码中dvbm由vmm当前时刻的速度增量，rotm和scullm构成。

rotm称为速度的旋转误差补偿量在书83页：

 scullm为划桨误差补偿量在n=1时，没有补偿。

 phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts; 

 Kg和Ka应该是用来校正的，实际陀螺和加速度计并不是正交的，代码中都为1。

ins.eb和ins.db分别为陀螺常值偏置和加速度常值偏置。

下面进行外推：

imu更新时间为0.01s，单子样的nts=0.01；nts2=0.005；

速度更新、位置更新：根据前一时刻的速度和位置，外推得到nts时刻的速度、位置。得到区间中点的状态。

vn01 = ins.vn+ins.an*nts2; 
pos01 = ins.pos+ins.Mpv*vn01*nts2;

 Mpv形式如下：

 下面计算下列参数：

 在速度更新下：

根据枯荣有常大佬写的：

 只不过，我们这边的单子样没有划桨补偿，只有前两项。

下面把载体系下的比力转换到导航系下:

ins.fn = qmulv(ins.qnb, ins.fb);

 下面计算中间时刻去掉地球重力加速度的导航坐标系的加速度

ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;

ins.anbar = 0.9*ins.anbar + 0.1*ins.an;这个暂时不知道；

接下来用中间时刻的加速度来计算整个时间段的速度增量，得到当前时刻的速度，此方法应该叫中值法速度更新：

vn1 = ins.vn + ins.an*nts;

 接下来进行位置更新：

首先把导航系下的速度转换到经纬高为单位的速度

ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
ins.pos = ins.pos + ins.Mpvvn*nts;  

计算出中间时刻的速度，乘以时间求出经纬度位置的变化量，加上起始时刻的位置，得到当前时刻的位置。

下面进行的是姿态更新书80页：

这个是连续的，离散化的应该是左边为当前时刻的姿态，右边为载体系相对于惯性系的角增量和导航系相对于惯性系的角增量。

下面为用当前时刻的qnb对参数进行更新： 

[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
ins.avp = [ins.att; ins.vn; ins.pos];

上述完成了惯导的更新，下面进入了卡尔曼滤波部分

第一步构造连续状态下的Ft：

依据为严老师书P94页的误差方程：

 代码构造如下形式:

%%     fi     dvn    dpos    eb       db
Ft = [ Maa    Mav    Map    -ins.Cnb  O33 
       Mva    Mvv    Mvp     O33      ins.Cnb 
       O33    Mpv    Mpp     O33      O33
       zeros(6,9)  diag(-1./[ins.tauG;ins.tauA]) ];

%     Maa = [ 0,               ins.eth.wnin(3),-ins.eth.wnin(2); 
%            -ins.eth.wnin(3), 0,               ins.eth.wnin(1); 
%             ins.eth.wnin(2),-ins.eth.wnin(1), 0 ];

%     Mav = [ 0,       -f_RMh, 0;
%             f_RNh,    0,     0;
%             f_RNh*tl, 0,     0 ];

Map = Mp1+Mp2;

%     Mp1 = [ 0,               0, 0;
%            -ins.eth.wnie(3), 0, 0;
%             ins.eth.wnie(2), 0, 0 ];

%     Mp2 = [ 0,             0,  vN_RMh2;
%             0,             0, -vE_RNh2;
%             vE_clRNh*secl, 0, -vE_RNh2*tl];



%     Mva = [ 0,         -ins.fn(3),   ins.fn(2);                   
%             ins.fn(3),  0,           -ins.fn(1);
%            -ins.fn(2),  ins.fn(1),          0 ];

 Mvv = Avn*Mav - Awn;
 Mvp = Avn*(Mp1+Map);

%     Mpv = [ 0,       f_RMh, 0;
%             f_clRNh, 0,     0;
%             0,       0,     1 ];

%     Mpp = [ 0,           0, -vN_RMh2;
%             vE_clRNh*tl, 0, -vE_RNh2*secl;
%             0,           0,  0 ];

 见严老师书推导：

Avn推导如下：

 

g0 = 9.7803267714;  scl = ins.eth.sl*ins.eth.cl;
Mvp(3) = Mvp(3)-g0*(5.27094e-3*2+2.32718e-5*4*ins.eth.sl2)*scl;
Mvp(9) = Mvp(9)+3.086e-6; 

 mvp(3)和mvp(9)不太明白

上述完成F的构造；

下面根据GPS数据进行滤波更新。

至此惯导更新部分大部分已经完成，后续还会有一些详细推导，和卡尔曼滤波部分。

上述为本人看法，若有不对请指正。

 -----------------------------------------------------------未完待续---------------------------------------------------------

主要参考资料为严老师的捷联惯导算法与组合导航原理

感谢严老师的代码