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

本文链接：https://blog.youkuaiyun.com/wuwuku123/article/details/105361651

导航坐标系：东-北-天

载体坐标系：右-前-上

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

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

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

欧拉角微分方程：

二、四阶龙格-库塔算法;

代码如下：

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];

三、结果对比：

双子样算法求解：