登录 注册
开源 企业版 高校版 私有云 Gitee AI NEW
扫描微信二维码支付
取消
支付完成
Watch
不关注 关注所有动态 仅关注版本发行动态 关注但不提醒动态
1 Star 3 Fork 1

mmmmachine/SINS-GPS-EKF

代码 Issues 0 Pull Requests 0 Wiki 统计 流水线
服务
加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目，私有仓库也完全免费 ：）
免费加入
该仓库未声明开源许可证文件（LICENSE），使用请关注具体项目描述及其代码上游依赖。
项目仓库所选许可证以仓库主分支所使用许可证为准
克隆/下载
CovariancePrediction.m 44.23 KB
一键复制 编辑 原始数据 按行查看 历史
YangYue 提交于 2018-03-30 11:56 . SINS/GPS EKF
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273
%#codegen

function nextP  = CovariancePrediction(deltaAngle, ...

    deltaVelocity, ...

    states,...

    P, ...  % Previous state covariance matrix

    dt) % IMU and prediction time step





    

    dAngBiasSigma = single(dt*0.05/3600*pi/180);

    dVelBiasSigma = single(dt*0.01/60);

%     magEarthSigma = single(dt*10/60); % 10 mgauss per minute

%     magBodySigma  = single(dt*100/60); % 100 mgauss per minute - allow to adapt more rapidly due to changes in airframe

    

    processNoise = [1e-9*ones(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1]].^2;

    

    % Specify the estimated errors on the delta angles and delta velocities

    daxCov = (dt*25.0/60*pi/180)^2;

    dayCov = (dt*25.0/60*pi/180)^2;

    dazCov = (dt*25.0/60*pi/180)^2;

    dvxCov = (dt*0.5)^2;

    dvyCov = (dt*0.5)^2;

    dvzCov = (dt*0.5)^2;

    

    dvx = deltaVelocity(1);

    dvy = deltaVelocity(2);

    dvz = deltaVelocity(3);

    dax = deltaAngle(1);

    day = deltaAngle(2);

    daz = deltaAngle(3);

    

    q0 = states(1);

    q1 = states(2);

    q2 = states(3);

    q3 = states(4);

    

%     vn = states(5);

%     ve = states(6);

%     vd = states(7);

%     

%     pn = states(8);

%     pe = states(9);

%     pd = states(10);

    

    dax_b = states(11);

    day_b = states(12);

    daz_b = states(13);

    

    dvx_b = states(14);

    dvy_b = states(15);

    dvz_b = states(16);

    

%     

%     magN = states(17);

%     magE = states(18);

%     magD = states(19);

%     

%     magX = states(20);

%     magY = states(21);

%     magZ = states(22);

    

    % Predicted covariance

SF = zeros(14,1);

SF(1) = dvz - dvz_b;

SF(2) = dvy - dvy_b;

SF(3) = dvx - dvx_b;

SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1);

SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3);

SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2);

SF(7) = day/2 - day_b/2;

SF(8) = daz/2 - daz_b/2;

SF(9) = dax/2 - dax_b/2;

SF(10) = 2*q1*SF(2);

SF(11) = 2*q0*SF(1);

SF(12) = q1/2;

SF(13) = q2/2;

SF(14) = q3/2;



SG = zeros(8,1);

SG(1) = q0/2;

SG(2) = q3^2;

SG(3) = q2^2;

SG(4) = q1^2;

SG(5) = q0^2;

SG(6) = 2*q2*q3;

SG(7) = 2*q1*q3;

SG(8) = 2*q1*q2;



SQ = zeros(11,1);

SQ(1) = dvzCov*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyCov*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxCov*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3);

SQ(2) = dvzCov*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxCov*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyCov*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3);

SQ(3) = dvzCov*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyCov*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxCov*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5));

SQ(4) = (dayCov*q1*SG(1))/2 - (dazCov*q1*SG(1))/2 - (daxCov*q2*q3)/4;

SQ(5) = (dazCov*q2*SG(1))/2 - (daxCov*q2*SG(1))/2 - (dayCov*q1*q3)/4;

SQ(6) = (daxCov*q3*SG(1))/2 - (dayCov*q3*SG(1))/2 - (dazCov*q1*q2)/4;

SQ(7) = (daxCov*q1*q2)/4 - (dazCov*q3*SG(1))/2 - (dayCov*q1*q2)/4;

SQ(8) = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG(1))/2;

SQ(9) = (dayCov*q2*q3)/4 - (daxCov*q1*SG(1))/2 - (dazCov*q2*q3)/4;

SQ(10) = SG(1)^2;



SPP = zeros(8,1);

SPP(1) = SF(10) + SF(11) - 2*q2*SF(3);

SPP(2) = q0^2 - q1^2 + q2^2 - q3^2;

SPP(3) = q0^2 + q1^2 - q2^2 - q3^2;

SPP(4) = 2*q0*q2 - 2*q1*q3;

SPP(5) = 2*q0*q1 - 2*q2*q3;

SPP(6) = 2*q0*q3 - 2*q1*q2;

SPP(7) = SF(8);

SPP(8) = SF(9);



nextP = zeros(16,16);

nextP(1,1) = P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7) + (daxCov*SQ(11))/4 - SF(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(12)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(13)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SF(14)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(8)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + (dayCov*q2^2)/4 + (dazCov*q3^2)/4;

nextP(1,2) = P(1,2) + SQ(9) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7) - SF(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SF(14)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) - SF(13)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) + SPP(8)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)))/2;

nextP(2,2) = P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) + daxCov*SQ(10) - (P(11,2)*q0)/2 - SF(7)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SF(14)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) - SF(13)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) + SPP(8)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(7)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + (dayCov*q3^2)/4 + (dazCov*q2^2)/4 - (q0*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2))/2;

nextP(1,3) = P(1,3) + SQ(8) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7) + SF(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SF(14)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(12)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SPP(8)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - (q0*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)))/2;

nextP(2,3) = P(2,3) + SQ(6) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2 + SF(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SF(14)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) + SF(12)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) - SPP(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SPP(8)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - (q0*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2))/2;

nextP(3,3) = P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) + dayCov*SQ(10) + (dazCov*SQ(11))/4 - (P(12,3)*q0)/2 + SF(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SF(14)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) + SF(12)*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2) - SPP(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SPP(8)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + (daxCov*q3^2)/4 - (q0*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2))/2;

nextP(1,4) = P(1,4) + SQ(7) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7) + SF(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SF(13)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) - SF(12)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SPP(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SPP(8)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)))/2;

nextP(2,4) = P(2,4) + SQ(5) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2 + SF(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SF(13)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) - SF(12)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) + SPP(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SPP(8)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - (q0*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2))/2;

nextP(3,4) = P(3,4) + SQ(4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2 + SF(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SF(13)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) - SF(12)*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2) + SPP(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SPP(8)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - (q0*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2))/2;

nextP(4,4) = P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) + (dayCov*SQ(11))/4 + dazCov*SQ(10) - (P(13,4)*q0)/2 + SF(7)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) + SF(13)*(P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2) - SF(12)*(P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2) + SPP(7)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) - SPP(8)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + (daxCov*q2^2)/4 - (q0*(P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2))/2;

nextP(1,5) = P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7) + SF(6)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(5)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - SPP(3)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) + SPP(6)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) - (2*q0*q2 + 2*q1*q3)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7));

nextP(2,5) = P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (2*q0*q2 + 2*q1*q3)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2) - (P(11,5)*q0)/2 + SF(6)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(5)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - SPP(3)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) + SPP(6)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2);

nextP(3,5) = P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2) - (P(12,5)*q0)/2 + SF(6)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(5)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - SPP(3)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) + SPP(6)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2);

nextP(4,5) = P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2) - (P(13,5)*q0)/2 + SF(6)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(5)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) - SPP(3)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) + SPP(6)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2);

nextP(5,5) = P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) + dvyCov*(SG(8) - 2*q0*q3)^2 + dvzCov*(SG(7) + 2*q0*q2)^2 - (2*q0*q2 + 2*q1*q3)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) + dvxCov*(SG(2) + SG(3) - SG(4) - SG(5))^2 - P(16,5)*(2*q0*q2 + 2*q1*q3) + SF(6)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(5)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) - SPP(3)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) + SPP(6)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3));

nextP(1,6) = P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7) + SF(5)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(6)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - SPP(1)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(2)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) + SPP(5)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q3 + 2*q1*q2)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7));

nextP(2,6) = P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (2*q0*q3 + 2*q1*q2)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (P(11,6)*q0)/2 + SF(5)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(6)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - SPP(1)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SPP(2)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) + SPP(5)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);

nextP(3,6) = P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (P(12,6)*q0)/2 + SF(5)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(6)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) - SPP(1)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SPP(2)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) + SPP(5)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);

nextP(4,6) = P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (P(13,6)*q0)/2 + SF(5)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(6)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) - SPP(1)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SPP(2)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) + SPP(5)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);

nextP(5,6) = P(5,6) + SQ(3) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - (2*q0*q3 + 2*q1*q2)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) - P(16,6)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(6)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) - SPP(1)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SPP(2)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) + SPP(5)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3));

nextP(6,6) = P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) + dvxCov*(SG(8) + 2*q0*q3)^2 + dvzCov*(SG(6) - 2*q0*q1)^2 - (2*q0*q3 + 2*q1*q2)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2)) + dvyCov*(SG(2) - SG(3) + SG(4) - SG(5))^2 - P(14,6)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(6)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) - SPP(1)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SPP(2)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) + SPP(5)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2));

nextP(1,7) = P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7) + SF(5)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(6)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(4)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(4)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q1 + 2*q2*q3)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7));

nextP(2,7) = P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (2*q0*q1 + 2*q2*q3)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) - (P(11,7)*q0)/2 + SF(5)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(6)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(4)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(4)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);

nextP(3,7) = P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) - (P(12,7)*q0)/2 + SF(5)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(6)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(4)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SPP(4)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);

nextP(4,7) = P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) - (P(13,7)*q0)/2 + SF(5)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(6)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(4)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SPP(4)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);

nextP(5,7) = P(5,7) + SQ(2) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) - (2*q0*q1 + 2*q2*q3)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) - P(16,7)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(6)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SPP(4)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3));

nextP(6,7) = P(6,7) + SQ(1) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2)) - (2*q0*q1 + 2*q2*q3)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) - P(14,7)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SF(6)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) + SPP(1)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SPP(4)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2));

nextP(7,7) = P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) + SF(5)*(P(7,2) + P(2,2)*SF(5) - P(3,2)*SF(6) + P(4,2)*SF(4) + P(1,2)*SPP(1) + P(14,2)*SPP(4) - P(16,2)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,2)*(2*q0*q1 + 2*q2*q3)) - SF(6)*(P(7,3) + P(2,3)*SF(5) - P(3,3)*SF(6) + P(4,3)*SF(4) + P(1,3)*SPP(1) + P(14,3)*SPP(4) - P(16,3)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,3)*(2*q0*q1 + 2*q2*q3)) + SF(4)*(P(7,4) + P(2,4)*SF(5) - P(3,4)*SF(6) + P(4,4)*SF(4) + P(1,4)*SPP(1) + P(14,4)*SPP(4) - P(16,4)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,4)*(2*q0*q1 + 2*q2*q3)) + SPP(1)*(P(7,1) + P(2,1)*SF(5) - P(3,1)*SF(6) + P(4,1)*SF(4) + P(1,1)*SPP(1) + P(14,1)*SPP(4) - P(16,1)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,1)*(2*q0*q1 + 2*q2*q3)) + SPP(4)*(P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3)) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) + dvxCov*(SG(7) - 2*q0*q2)^2 + dvyCov*(SG(6) + 2*q0*q1)^2 - (q0^2 - q1^2 - q2^2 + q3^2)*(P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3)) + dvzCov*(SG(2) - SG(3) - SG(4) + SG(5))^2 - P(15,7)*(2*q0*q1 + 2*q2*q3) - (2*q0*q1 + 2*q2*q3)*(P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3));

nextP(1,8) = P(1,8) - P(3,8)*SF(7) + P(11,8)*SF(12) + P(12,8)*SF(13) + P(13,8)*SF(14) - P(2,8)*SPP(8) - P(4,8)*SPP(7) + dt*(P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7));

nextP(2,8) = P(2,8) - P(4,8)*SF(7) + P(12,8)*SF(14) - P(13,8)*SF(13) + P(1,8)*SPP(8) + P(3,8)*SPP(7) - (P(11,8)*q0)/2 + dt*(P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (P(11,5)*q0)/2);

nextP(3,8) = P(3,8) + P(1,8)*SF(7) - P(11,8)*SF(14) + P(13,8)*SF(12) - P(2,8)*SPP(7) + P(4,8)*SPP(8) - (P(12,8)*q0)/2 + dt*(P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (P(12,5)*q0)/2);

nextP(4,8) = P(4,8) + P(2,8)*SF(7) + P(11,8)*SF(13) - P(12,8)*SF(12) + P(1,8)*SPP(7) - P(3,8)*SPP(8) - (P(13,8)*q0)/2 + dt*(P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (P(13,5)*q0)/2);

nextP(5,8) = P(5,8) + P(1,8)*SF(6) + P(2,8)*SF(4) - P(4,8)*SF(5) + P(3,8)*SPP(1) - P(14,8)*SPP(3) + P(15,8)*SPP(6) + dt*(P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) - P(16,5)*(2*q0*q2 + 2*q1*q3)) - P(16,8)*(2*q0*q2 + 2*q1*q3);

nextP(6,8) = P(6,8) + P(1,8)*SF(5) + P(3,8)*SF(4) + P(4,8)*SF(6) - P(2,8)*SPP(1) - P(15,8)*SPP(2) + P(16,8)*SPP(5) + dt*(P(6,5) + P(1,5)*SF(5) + P(3,5)*SF(4) + P(4,5)*SF(6) - P(2,5)*SPP(1) - P(15,5)*SPP(2) + P(16,5)*SPP(5) - P(14,5)*(2*q0*q3 + 2*q1*q2)) - P(14,8)*(2*q0*q3 + 2*q1*q2);

nextP(7,8) = P(7,8) + P(2,8)*SF(5) - P(3,8)*SF(6) + P(4,8)*SF(4) + P(1,8)*SPP(1) + P(14,8)*SPP(4) - P(16,8)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,5) + P(2,5)*SF(5) - P(3,5)*SF(6) + P(4,5)*SF(4) + P(1,5)*SPP(1) + P(14,5)*SPP(4) - P(16,5)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,5)*(2*q0*q1 + 2*q2*q3)) - P(15,8)*(2*q0*q1 + 2*q2*q3);

nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt);

nextP(1,9) = P(1,9) - P(3,9)*SF(7) + P(11,9)*SF(12) + P(12,9)*SF(13) + P(13,9)*SF(14) - P(2,9)*SPP(8) - P(4,9)*SPP(7) + dt*(P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7));

nextP(2,9) = P(2,9) - P(4,9)*SF(7) + P(12,9)*SF(14) - P(13,9)*SF(13) + P(1,9)*SPP(8) + P(3,9)*SPP(7) - (P(11,9)*q0)/2 + dt*(P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (P(11,6)*q0)/2);

nextP(3,9) = P(3,9) + P(1,9)*SF(7) - P(11,9)*SF(14) + P(13,9)*SF(12) - P(2,9)*SPP(7) + P(4,9)*SPP(8) - (P(12,9)*q0)/2 + dt*(P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (P(12,6)*q0)/2);

nextP(4,9) = P(4,9) + P(2,9)*SF(7) + P(11,9)*SF(13) - P(12,9)*SF(12) + P(1,9)*SPP(7) - P(3,9)*SPP(8) - (P(13,9)*q0)/2 + dt*(P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (P(13,6)*q0)/2);

nextP(5,9) = P(5,9) + P(1,9)*SF(6) + P(2,9)*SF(4) - P(4,9)*SF(5) + P(3,9)*SPP(1) - P(14,9)*SPP(3) + P(15,9)*SPP(6) + dt*(P(5,6) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - P(16,6)*(2*q0*q2 + 2*q1*q3)) - P(16,9)*(2*q0*q2 + 2*q1*q3);

nextP(6,9) = P(6,9) + P(1,9)*SF(5) + P(3,9)*SF(4) + P(4,9)*SF(6) - P(2,9)*SPP(1) - P(15,9)*SPP(2) + P(16,9)*SPP(5) + dt*(P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) - P(14,6)*(2*q0*q3 + 2*q1*q2)) - P(14,9)*(2*q0*q3 + 2*q1*q2);

nextP(7,9) = P(7,9) + P(2,9)*SF(5) - P(3,9)*SF(6) + P(4,9)*SF(4) + P(1,9)*SPP(1) + P(14,9)*SPP(4) - P(16,9)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,6) + P(2,6)*SF(5) - P(3,6)*SF(6) + P(4,6)*SF(4) + P(1,6)*SPP(1) + P(14,6)*SPP(4) - P(16,6)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,6)*(2*q0*q1 + 2*q2*q3)) - P(15,9)*(2*q0*q1 + 2*q2*q3);

nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt);

nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt);

nextP(1,10) = P(1,10) - P(3,10)*SF(7) + P(11,10)*SF(12) + P(12,10)*SF(13) + P(13,10)*SF(14) - P(2,10)*SPP(8) - P(4,10)*SPP(7) + dt*(P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7));

nextP(2,10) = P(2,10) - P(4,10)*SF(7) + P(12,10)*SF(14) - P(13,10)*SF(13) + P(1,10)*SPP(8) + P(3,10)*SPP(7) - (P(11,10)*q0)/2 + dt*(P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (P(11,7)*q0)/2);

nextP(3,10) = P(3,10) + P(1,10)*SF(7) - P(11,10)*SF(14) + P(13,10)*SF(12) - P(2,10)*SPP(7) + P(4,10)*SPP(8) - (P(12,10)*q0)/2 + dt*(P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (P(12,7)*q0)/2);

nextP(4,10) = P(4,10) + P(2,10)*SF(7) + P(11,10)*SF(13) - P(12,10)*SF(12) + P(1,10)*SPP(7) - P(3,10)*SPP(8) - (P(13,10)*q0)/2 + dt*(P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (P(13,7)*q0)/2);

nextP(5,10) = P(5,10) + P(1,10)*SF(6) + P(2,10)*SF(4) - P(4,10)*SF(5) + P(3,10)*SPP(1) - P(14,10)*SPP(3) + P(15,10)*SPP(6) + dt*(P(5,7) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - P(16,7)*(2*q0*q2 + 2*q1*q3)) - P(16,10)*(2*q0*q2 + 2*q1*q3);

nextP(6,10) = P(6,10) + P(1,10)*SF(5) + P(3,10)*SF(4) + P(4,10)*SF(6) - P(2,10)*SPP(1) - P(15,10)*SPP(2) + P(16,10)*SPP(5) + dt*(P(6,7) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - P(14,7)*(2*q0*q3 + 2*q1*q2)) - P(14,10)*(2*q0*q3 + 2*q1*q2);

nextP(7,10) = P(7,10) + P(2,10)*SF(5) - P(3,10)*SF(6) + P(4,10)*SF(4) + P(1,10)*SPP(1) + P(14,10)*SPP(4) - P(16,10)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,7)*(2*q0*q1 + 2*q2*q3)) - P(15,10)*(2*q0*q1 + 2*q2*q3);

nextP(8,10) = P(8,10) + P(5,10)*dt + dt*(P(8,7) + P(5,7)*dt);

nextP(9,10) = P(9,10) + P(6,10)*dt + dt*(P(9,7) + P(6,7)*dt);

nextP(10,10) = P(10,10) + P(7,10)*dt + dt*(P(10,7) + P(7,7)*dt);

nextP(1,11) = P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7);

nextP(2,11) = P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2;

nextP(3,11) = P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2;

nextP(4,11) = P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2;

nextP(5,11) = P(5,11) + P(1,11)*SF(6) + P(2,11)*SF(4) - P(4,11)*SF(5) + P(3,11)*SPP(1) - P(14,11)*SPP(3) + P(15,11)*SPP(6) - P(16,11)*(2*q0*q2 + 2*q1*q3);

nextP(6,11) = P(6,11) + P(1,11)*SF(5) + P(3,11)*SF(4) + P(4,11)*SF(6) - P(2,11)*SPP(1) - P(15,11)*SPP(2) + P(16,11)*SPP(5) - P(14,11)*(2*q0*q3 + 2*q1*q2);

nextP(7,11) = P(7,11) + P(2,11)*SF(5) - P(3,11)*SF(6) + P(4,11)*SF(4) + P(1,11)*SPP(1) + P(14,11)*SPP(4) - P(16,11)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,11)*(2*q0*q1 + 2*q2*q3);

nextP(8,11) = P(8,11) + P(5,11)*dt;

nextP(9,11) = P(9,11) + P(6,11)*dt;

nextP(10,11) = P(10,11) + P(7,11)*dt;

nextP(11,11) = P(11,11);

nextP(1,12) = P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7);

nextP(2,12) = P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2;

nextP(3,12) = P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2;

nextP(4,12) = P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2;

nextP(5,12) = P(5,12) + P(1,12)*SF(6) + P(2,12)*SF(4) - P(4,12)*SF(5) + P(3,12)*SPP(1) - P(14,12)*SPP(3) + P(15,12)*SPP(6) - P(16,12)*(2*q0*q2 + 2*q1*q3);

nextP(6,12) = P(6,12) + P(1,12)*SF(5) + P(3,12)*SF(4) + P(4,12)*SF(6) - P(2,12)*SPP(1) - P(15,12)*SPP(2) + P(16,12)*SPP(5) - P(14,12)*(2*q0*q3 + 2*q1*q2);

nextP(7,12) = P(7,12) + P(2,12)*SF(5) - P(3,12)*SF(6) + P(4,12)*SF(4) + P(1,12)*SPP(1) + P(14,12)*SPP(4) - P(16,12)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,12)*(2*q0*q1 + 2*q2*q3);

nextP(8,12) = P(8,12) + P(5,12)*dt;

nextP(9,12) = P(9,12) + P(6,12)*dt;

nextP(10,12) = P(10,12) + P(7,12)*dt;

nextP(11,12) = P(11,12);

nextP(12,12) = P(12,12);

nextP(1,13) = P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7);

nextP(2,13) = P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2;

nextP(3,13) = P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2;

nextP(4,13) = P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2;

nextP(5,13) = P(5,13) + P(1,13)*SF(6) + P(2,13)*SF(4) - P(4,13)*SF(5) + P(3,13)*SPP(1) - P(14,13)*SPP(3) + P(15,13)*SPP(6) - P(16,13)*(2*q0*q2 + 2*q1*q3);

nextP(6,13) = P(6,13) + P(1,13)*SF(5) + P(3,13)*SF(4) + P(4,13)*SF(6) - P(2,13)*SPP(1) - P(15,13)*SPP(2) + P(16,13)*SPP(5) - P(14,13)*(2*q0*q3 + 2*q1*q2);

nextP(7,13) = P(7,13) + P(2,13)*SF(5) - P(3,13)*SF(6) + P(4,13)*SF(4) + P(1,13)*SPP(1) + P(14,13)*SPP(4) - P(16,13)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,13)*(2*q0*q1 + 2*q2*q3);

nextP(8,13) = P(8,13) + P(5,13)*dt;

nextP(9,13) = P(9,13) + P(6,13)*dt;

nextP(10,13) = P(10,13) + P(7,13)*dt;

nextP(11,13) = P(11,13);

nextP(12,13) = P(12,13);

nextP(13,13) = P(13,13);

nextP(1,14) = P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7);

nextP(2,14) = P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2;

nextP(3,14) = P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2;

nextP(4,14) = P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2;

nextP(5,14) = P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3);

nextP(6,14) = P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2);

nextP(7,14) = P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3);

nextP(8,14) = P(8,14) + P(5,14)*dt;

nextP(9,14) = P(9,14) + P(6,14)*dt;

nextP(10,14) = P(10,14) + P(7,14)*dt;

nextP(11,14) = P(11,14);

nextP(12,14) = P(12,14);

nextP(13,14) = P(13,14);

nextP(14,14) = P(14,14);

nextP(1,15) = P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7);

nextP(2,15) = P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2;

nextP(3,15) = P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2;

nextP(4,15) = P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2;

nextP(5,15) = P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3);

nextP(6,15) = P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2);

nextP(7,15) = P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3);

nextP(8,15) = P(8,15) + P(5,15)*dt;

nextP(9,15) = P(9,15) + P(6,15)*dt;

nextP(10,15) = P(10,15) + P(7,15)*dt;

nextP(11,15) = P(11,15);

nextP(12,15) = P(12,15);

nextP(13,15) = P(13,15);

nextP(14,15) = P(14,15);

nextP(15,15) = P(15,15);

nextP(1,16) = P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7);

nextP(2,16) = P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2;

nextP(3,16) = P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2;

nextP(4,16) = P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2;

nextP(5,16) = P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3);

nextP(6,16) = P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2);

nextP(7,16) = P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3);

nextP(8,16) = P(8,16) + P(5,16)*dt;

nextP(9,16) = P(9,16) + P(6,16)*dt;

nextP(10,16) = P(10,16) + P(7,16)*dt;

nextP(11,16) = P(11,16);

nextP(12,16) = P(12,16);

nextP(13,16) = P(13,16);

nextP(14,16) = P(14,16);

nextP(15,16) = P(15,16);

nextP(16,16) = P(16,16);

    

    % Add the process noise

    for i = 1:16

        nextP(i,i) = nextP(i,i) + processNoise(i);

    end

    

    % If the total position variance exceds 1E6 (1000m), then stop covariance

    % growth by setting the predicted to the previous values

    % This prevent an ill conditioned matrix from occurring for long periods

    % without GPS

    if (P(8,8) + P(9,9)) > 1E6

        nextP(8:9,:) = P(8:9,:);

        nextP(:,8:9) = P(:,8:9);

    end

    

    % Force symmetry on the covariance matrix to prevent ill-conditioning

    % of the matrix which would cause the filter to blow-up

    for i = 2:16

        for j = 1:(i-1)

            temp = 0.5*(nextP(i,j) + nextP(j,i));

            nextP(i,j) = temp;

            nextP(j,i) = temp;

        end

    end

   

end
马建仓 AI 助手
尝试更多
代码解读
代码找茬
代码优化
Matlab
1
https://gitee.com/mmmmachine/SINS-GPS-EKF.git
git@gitee.com:mmmmachine/SINS-GPS-EKF.git
mmmmachine
SINS-GPS-EKF
SINS-GPS-EKF
master
点此查找更多帮助

搜索帮助

Git 命令在线学习 如何在 Gitee 导入 GitHub 仓库
Git 仓库基础操作
企业版和社区版功能对比
SSH 公钥设置
如何处理代码冲突
仓库体积过大，如何减小？
如何找回被删除的仓库数据
Gitee 产品配额说明
GitHub仓库快速导入Gitee及同步更新
什么是 Release（发行版）
将 PHP 项目自动发布到 packagist.org
评论
仓库举报
回到顶部