Psins代码解析之四阶龙格-库塔（test_SINS_Runge_Kutta.m）&双子样算法

系统管理员 2023-07-23 05:57 6阅读 0赞

导航坐标系：东-北-天

载体坐标系：右-前-上

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

### **一、欧拉角速率和Wbnb之间关系** ###

从地理坐标系到载体坐标系的方向余弦矩阵为：通过欧拉角进行变换：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

欧拉角微分方程：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

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

代码如下：

for k=1:nn:len-nn+1-1
        k1 = k+nn-1;  
        wvm = imu(k:k1,1:6);  t = imu(k1,end);
        ins = insupdate(ins, wvm);  ins.pos(3) = trj.avp(k1,9); %高度值始终为参考初始高度
        wf = imu(k:k1+1,1:6)'/ts; %变为速率形式，3个采样值
        avp_rk4 = rgkt4(@dsins, wf, avp_rk4, nts);  avp_rk4(9) = trj.avp(k1,9); %高度值始终为参考初始高度
        avp(ki,:) = [ins.avp; t]';
        avp4(ki,:) = [avp_rk4; t]';
        ki = timebar; 
    end

其中龙格库塔计算程序为：

x0 = x01(:,1);  xhalf = x01(:,2);  x1 = x01(:,3);
        k1 = hfun(x0,    y0);
        k2 = hfun(xhalf, y0+h/2*k1);
        k3 = hfun(xhalf, y0+h/2*k2);
        k4 = hfun(x1,    y0+h*k3);
        y1 = y0 + h/6*(k1+2*(k2+k3)+k4);

计算欧拉角速率、速度变化率（加速度）、位置变化率，为

函数原型：dsins.m

% Prototype: davp = dsins(wf, avp)
    % Inputs: wf - wf = [wb, fb], wb for gyro angular rate and fb for acc
    %              specific force
    %         avp - avp = [att, vn, pos], Euler angles, velocity & position
    % Output: avp - d(avp)/dt

代码实现：

wb = wf(1:3);  fb = wf(4:6);
        att = avp(1:3);  vn = avp(4:6);  pos = avp(7:9);
        s = sin(att); c = cos(att);
        si = s(1); sj = s(2); sk = s(3);
        ci = c(1); cj = c(2); ck = c(3);  ti = si/ci;
        Caw = [  cj,     0,  sj;
                 sj*ti,  1, -cj*ti;
                -sj/ci,  0,  cj/ci ];  % a2caw
        Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
                cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
               -ci*sj,           si,     ci*cj           ]; % a2mat
        eth = earth(pos, vn);
        datt = Caw*(wb-Cnb'*eth.wnin); %欧拉角变化速率
        dvn = Cnb*fb + eth.gcc;
        dpos = [vn(2)/eth.RMh;vn(1)/eth.clRNh;vn(3)];
        davp = [datt; dvn; dpos];

三、结果对比：

双子样算法求解：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5][]

双子样求解avp和参考振值误差：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6][]

四阶龙格-库塔求解avp:

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7][]

四阶龙格-库塔求解avp与参考真值差：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8][]

两种方法所求的avp与参考真值之差，合并画图：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9][]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20230528/78151d5df20a44fc97cabb440ff78a93.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: /images/20230528/80c3a8774b7b43ddbd22b576ba129c72.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2]: /images/20230528/3ccbea29cec44f4db6efa62a074dbe67.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3]: /images/20230528/3f0149af059e48538efc037a9c30d433.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4]: /images/20230528/5173bb3c03d74036b6bf27e1367212dd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5]: /images/20230528/af817b76472e468e81e82e1844304eb1.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 6]: /images/20230528/7cdd9525801747cd8fcd53745cc5d035.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 7]: /images/20230528/9285b56699094255a43eef7f6a66db06.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 8]: /images/20230528/5e32ccef6b4d413cae8de8438b1466d9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 9]: /images/20230528/7d501a3ace1a4a8d8ef4372833692ac3.png