Psins代码解析之粗对准(test_align_methods_compare.m)&精对准

柔情只为你懂 2023-07-24 08:31 12阅读 0赞

*  ### **背景：** ###

导航坐标系：东-北-天

载体坐标系：右-前-上

欧拉角定义：3-1-2旋转，（航向角-俯仰角-滚转角），其中航向角北偏西为正，范围【-pi pi】

地球自转引起的导航系旋转：

![20200407135547349.png][]

因地球表面弯曲，载体在地球表面运动，导致导航系旋转：

![2020040713570776.png][]

重力矢量在地理坐标系的投影为：

![20200410112158667.png][]

*  ### **对准条件：** ###

初始对准一般是在运载体对地静止的环境下进行的， 即运载体相对地面既没有明显的线运动也没有角运动，且对准地点处的地理位置准确已知， 也就是说重力矢量 g 和地球自转角速度矢量ωie 在地理坐标系（初始对准参考坐标系）的分量准确已知，分别如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

# **一、解析粗对准** alignsb.m #

### 1、捷联惯导姿态微分方程和比力方程为： ###

![20200410113545523.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

又因为：

![20200410113932581.png][]

![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][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4][]

**最终计算公式如下：**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5][]

**对准的极限精度为：**

![20200410144210567.png][]

即：水平失准角的对准误差取决于加速度计的等效水平测量误差![2020041014460654.png][]

方位失准角的对准误差取决于陀螺的等效东向测量误差![20200410144706734.png][]

\[-imuerr.db(2);imuerr.db(1)\]/glv.g0;-imuerr.eb(1)/(cos(avp(7))\*glv.wie)\]

*  **仿真数据实现：**

glvs
    ts = 0.1;   % sampling interval
    T = 1000; %总时长1000秒、即16分钟.40秒
    avp0 = avpset([0;0;0], [0;0;0], [30;108;380]); %初始姿态为0
    imuerr = imuerrset(0.01, 100, 0.001, 1);
    % imuerr = imuerrset(0.01, 100,0,0);
    imu = imustatic(avp0, ts, T, imuerr);   % IMU simulation
    davp = avpseterr([-30;30;30], [0.01;0.01;0.01]*0, [1;1;1]*0); %只存在初始姿态误差；速度、位置误差为0
    avp = avpadderr(avp0, davp);

*  **解析粗对准实现代码;**

%% coarse align
    [attsb, qnb] = alignsb(imu, avp(7:9));
    phi = [aa2phi(attsb,[0;0;0]), [[-imuerr.db(2);imuerr.db(1)]/glv.g0;-imuerr.eb(1)/(cos(avp(7))*glv.wie)]]

*  **其中初始姿态角为\[0 0 0 \]，结果如下：**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6][]

# **二、间接粗对准：** aligni0.m #

*  初始时刻载体惯性系（b0 ）： 与初始对准开始瞬时的载体坐标系（ b 系）重合， 随后相对于惯性空间无转动；
 *  初始时刻导航惯性系（n0 ）： 与初始对准开始瞬时的导航坐标系（ n 系，即地理坐标系）重合， 随后相对于惯性空间无转动；

间接初始对准方法的关键是求解b0 系与 n0 系的方位关系，即Cnobo。

### ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7][]   1、重力矢量在n0系上的投影为： ###

![20200410111938102.png][]

因为：

![20200410112513933.png][]  
![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8][]

### 2、加速度计的比力输出在b0系投影为： ###

![20200410112957999.png][]

因为 Cb0为惯性系，而且IMU的输出是基于惯性系，所以Wbb0b=Wbib;

![20200410113032196.png][]

根据前述微分方程，如下：

![20200410115206183.png][]

将方程7.1-21b两边同时左乘Cn0n，得：

![20200410115324227.png][]

最终计算如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9][]

# **三、Kalman精对准：**alignvn.m& alignfn.m #

### 1、alignvn.m 以速度为量测的Kalman精对准 ###

静基座下，捷联惯导速度更新解算即为速度误差，将其作为观测量，同时也是测量误差量；利用kalman量测方程完成对失准角的估计。

**（1）简化的姿态、速度更新算法：**

![20200410160912955.png][]

![20200410160927661.png][]

**（2）简化的姿态、速度误差方程：**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 10][]

**（3）初始对准状态空间模型：**

将陀螺仪和加速度计的常值零偏扩展为状态量，状态空间模型如下：

系统的状态量为：\[失准角、速度误差、陀螺仪逐次启动零偏稳定性、加速度计零偏\]

![20200410161419570.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 11][]

**（4）代码实现：**

**将粗对准得到的姿态角当作精对准的初始角度；**

wvm = imu(k:k+nn-1,1:6);
            [phim, dvbm] = cnscl(wvm);
            Cnb = q2mat(qnb);
            dvn = Cnn*Cnb*dvbm;
            vn = vn + dvn + eth.gn*nts;
            %qnb = qupdt(qnb, phim-Cnb'*eth.wnin*nts);
            qnb = qupdt2(qnb, phim, eth.wnin*nts);
            Cnbts = Cnb*nts;
            kf.Phikk_1(4:6,1:3) = askew(dvn);
            kf.Phikk_1(1:3,7:9) = -Cnbts; kf.Phikk_1(4:6,10:12) = Cnbts;
            kf = kfupdate(kf, vn);
            tag=0.4;tag1=1-tag;
            qnb = qdelphi(qnb, tag*kf.xk(1:3)); kf.xk(1:3) = tag1*kf.xk(1:3);
            vn = vn-tag*kf.xk(4:6);  kf.xk(4:6) = tag1*kf.xk(4:6);
            attk(ki,:) = [q2att(qnb)',imu(k+nn-1,end)];
            xkpk(ki,:) = [kf.xk; diag(kf.Pxk)];

function kf = avnkfinit(nts, pos, phi0, imuerr, wvn)
        eth = earth(pos); wnie = eth.wnie;
        kf = []; kf.s = 1; kf.nts = nts;
    	kf.Qk = diag([3*imuerr.web; 3*imuerr.wdb; zeros(6,1)])^2*nts;
        kf.Gammak = 1;
    	kf.Rk = diag(wvn)^2;
    	kf.Pxk = diag([phi0; [1;1;1]; imuerr.eb; imuerr.db])^2;
    	Ft = zeros(12); Ft(1:3,1:3) = askew(-wnie); kf.Phikk_1 = eye(12)+Ft*nts;
    	kf.Hk = [zeros(3),eye(3),zeros(3,6)];
        [kf.m, kf.n] = size(kf.Hk);
        kf.I = eye(kf.n);
        kf.xk = zeros(kf.n, 1);
        kf.adaptive = 0;
        kf.xconstrain = 0; kf.pconstrain = 0;
        kf.fading = 1;

**（5）结果实现：**

<table style="width:500px;"> 
 <tbody> 
  <tr> 
   <td>粗对准的结果（°）</td> 
   <td>精对准的结果（速度为观测量）（°）</td> 
  </tr> 
  <tr> 
   <td>0.005750544250598</td> 
   <td>&nbsp;0.005782319297423</td> 
  </tr> 
  <tr> 
   <td>-0.005835008798230</td> 
   <td>-0.005683278866238</td> 
  </tr> 
  <tr> 
   <td>0.038523957584313</td> 
   <td>0.045178296080427</td> 
  </tr> 
 </tbody> 
</table>

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 12][]

### 2、alignvfn.m 以比力为量测的Kalman精对准 ###

静基座下，加速度计输出**比力**作为观测量；测量误差量为：比力 - 速度误差方程；

**（1）加速度计输出比力和捷联惯导更新的速度作为量测，其主要区别在于转移矩阵和量测矩阵的建立；**

加速度计输出比力中，状态量为 \[phiE, phiN, phiU, eby, ebz\];

通常，静基座下，东、北向加速度计、东向陀螺仪是不可观的；因此，此处没有选择东向陀螺仪作为状态量；

*  **转移矩阵为：**

Ft = \[ 0 wU -wN 0 0  
\-wU 0 0 -1 0  
wN 0 0 0 -1  
zeros(2,5) \];

*  **量测矩阵为：**

kf.Hk = \[ 0 -g 0 0 0  
g 0 0 0 0 \];

量测方程的建立，根据速度误差方程和静基座下(近似)，fn=-gn；

![2020041017194829.png][]

![202004101717428.png][]

将上式误差状态方程展开如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 13][]

**（2）代码实现：**

*  状态转移矩阵和量测矩阵：

function kf = afnkfinit(nts, pos, phi0, imuerr)
        eth = earth(pos);
        kf = []; kf.s = 1; kf.nts = nts;
        kf.Qk = diag([imuerr.web; 0;0])^2*nts;
    	kf.Rk = diag(imuerr.wdb(1:2)/sqrt(nts))^2;
    	kf.Pxk = diag([phi0; imuerr.eb(1:2)])^2;
    	wN = eth.wnie(2); wU = eth.wnie(3); g = -eth.gn(3);
    	Ft = [  0   wU -wN   0   0 
               -wU  0   0   -1   0 
                wN  0   0    0  -1 
                zeros(2,5)          ];
        kf.Phikk_1 = eye(5)+Ft*nts;
        kf.Hk = [ 0  -g  0  0  0
                  g   0  0  0  0 ];
        [kf.m, kf.n] = size(kf.Hk);
        kf.I = eye(kf.n);
        kf.xk = zeros(kf.n, 1);
        kf.adaptive = 0;
        kf.fading = 1;
        kf.Gammak = 1;
        kf.xconstrain = 0;
        kf.pconstrain = 0;

*  Kalman滤波器时间和量测更新：

wvm = imu(k:k+nn-1, 1:6);
            [phim, dvbm] = cnscl(wvm);
            fn = Cnn*qmulv(qnb, dvbm/nts);
            qnb = qupdt(qnb, phim-qmulv(qconj(qnb),eth.wnie)*nts);  % att updating
            kf = kfupdate(kf, fn(1:2));
            tag=0.1;tag1=1-tag;
            qnb = qdelphi(qnb, tag*kf.xk(1:3)); kf.xk(1:3) = tag1*kf.xk(1:3); % feedback
            attk(ki,:) = q2att(qnb)';
            xkpk(ki,:) = [kf.xk; diag(kf.Pxk)];

**（3）结果实现：**

<table style="width:500px;"> 
 <tbody> 
  <tr> 
   <td>粗对准的结果（°）</td> 
   <td>精对准的结果（速度为观测量）（°）</td> 
  </tr> 
  <tr> 
   <td>0.005722167719818</td> 
   <td>0.005687376038254</td> 
  </tr> 
  <tr> 
   <td>-0.005724676591646</td> 
   <td>-0.005754972245868</td> 
  </tr> 
  <tr> 
   <td>&nbsp;0.029101368855361</td> 
   <td>0.030922843858521</td> 
  </tr> 
 </tbody> 
</table>

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 14][]

参考：

《捷联惯导系统静基座初始对准精度分析及仿真》

[20200407135547349.png]: /images/20230528/2070abbe48cd44a2941382b134725416.png
[2020040713570776.png]: /images/20230528/192300d4b15948d39dac19ba6e106a42.png
[20200410112158667.png]: /images/20230528/41a504df182649669e9214a830607b3b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20230528/c287037e11f74f75a531773e0c8200c3.png
[20200410113545523.png]: /images/20230528/fcea7cc697274425b2145ec24620977e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: /images/20230528/38af5b5f3e3d4c65b08dff1fb9236bb0.png
[20200410113932581.png]: /images/20230528/870461d69f77491fbfed83e3b8d6f5ed.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2]: /images/20230528/87493c8e63854444a4988329b8bd32c5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3]: /images/20230528/a9fc82f184b34391b40072d90c2f1bfa.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4]: /images/20230528/51d8e1e12caf42dbbfead488145ab0d6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5]: /images/20230528/280417ee22df46059c4c3a462cc6a2e0.png
[20200410144210567.png]: /images/20230528/902577aa74e54ef587807fa50cfa984c.png
[2020041014460654.png]: /images/20230528/4664f652ae3640cd8baf7621e690be8c.png
[20200410144706734.png]: /images/20230528/9a10f26df12943b7b657ed0085e91d9c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6]: /images/20230528/42fa52d900f8452d83562e8fdc28342a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7]: /images/20230528/e3377d75e8a94d4992ed6f1da25be8c8.png
[20200410111938102.png]: /images/20230528/ad8812e72e104e129aaf759d21d2d798.png
[20200410112513933.png]: /images/20230528/853e1d9d3c354e7a90a33fe08f8096b3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8]: /images/20230528/ff6b3a37b7804da1a1d916234a2f7acd.png
[20200410112957999.png]: /images/20230528/32391b0089d142c89515975ae41c0193.png
[20200410113032196.png]: /images/20230528/7be873ea5ce642a39fe189951137d8ee.png
[20200410115206183.png]: /images/20230528/cffdb590bbf94ec0a1ec8d960c7c2cff.png
[20200410115324227.png]: /images/20230528/43e07337c95a4463826bddb314b01b22.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9]: /images/20230528/0146815dae2f4b11b771d141fb5efcfe.png
[20200410160912955.png]: /images/20230528/a9de44cddc2e4de39b00708e92da9521.png
[20200410160927661.png]: /images/20230528/7c623bb966b84962822ceb00615e2547.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 10]: /images/20230528/993ebd4016894317a1e55170757ca521.png
[20200410161419570.png]: /images/20230528/b70a58dd71074f38997cc73f1b76e508.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 11]: /images/20230528/95d837b643384f6292898bd6c0d50d11.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 12]: /images/20230528/10b4db6b8b654a63b8fee13f8a75216e.png
[2020041017194829.png]: /images/20230528/428c1e354947448480ba2d9c96a156fa.png
[202004101717428.png]: /images/20230528/ae726722d01a4173bc02d5599b9750e0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 13]: /images/20230528/b1ebe35655904d88bc7d1bba4aa6bf92.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 14]: /images/20230528/a5a394ce46b343ccb7163483ccfaac59.png