Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Use alternate form for quaternion to rotmat equations in derivation

This commit is contained in:
Paul Riseborough 2021-07-12 07:49:57 +10:00 committed by Randy Mackay
parent 65d0dcf3b7
commit 4f8f187b81
2 changed files with 97 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AP_NavEKF3/derivation/generated/covariance_generated.cpp
View File

@ -190,10 +190,10 @@ const ftype PS186 = -PS165*P[6][14] + PS166*P[6][13] + PS60*P[0][6] + PS62*P[3][
nextP[0][0] = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
nextP[0][1] = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
nextP[1][1] = PS1*PS42 + PS11*PS80 - PS12*PS89 + PS13*PS82 + PS2*PS5 + PS3*PS4 - PS34*PS85 - PS7*PS87 + PS77*PS9 + PS92;
nextP[1][1] = PS1*PS95 + PS100*PS11 + PS102*PS13 - PS105*PS34 - PS107*PS7 - PS109*PS12 + PS112 + PS2*PS5 + PS3*PS4 + PS9*PS97;
nextP[0][2] = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5;
nextP[1][2] = PS1*PS40 + PS11*PS89 + PS12*PS80 - PS13*PS92 - PS3*PS40 - PS34*PS77 - PS39*PS5 + PS6*PS87 + PS82 - PS85*PS9;
nextP[2][2] = PS0*PS5 + PS1*PS4 + PS103*PS6 + PS104*PS11 + PS106*PS12 - PS108*PS34 - PS109*PS9 - PS111*PS13 + PS113 + PS3*PS42;
nextP[1][2] = PS1*PS40 + PS100*PS12 + PS102 - PS105*PS9 + PS107*PS6 + PS109*PS11 - PS112*PS13 - PS3*PS40 - PS34*PS97 - PS39*PS5;
nextP[2][2] = PS0*PS5 + PS1*PS4 + PS11*PS128 + PS12*PS130 + PS127*PS6 - PS13*PS135 - PS132*PS34 - PS133*PS9 + PS137 + PS3*PS95;
nextP[0][3] = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5;
nextP[1][3] = -PS1*PS38 - PS11*PS82 + PS12*PS92 + PS13*PS80 - PS3*PS37 - PS34*PS87 + PS38*PS5 - PS6*PS77 + PS7*PS85 + PS89;
nextP[2][3] = -PS1*PS35 - PS103*PS34 + PS104 + PS106*PS13 - PS108*PS6 + PS109*PS7 - PS11*PS113 + PS111*PS12 + PS3*PS36 - PS36*PS5;
@ -224,68 +224,68 @@ nextP[4][7] = -PS139*P[7][15] + PS140*P[7][14] + PS158*dt - PS44*P[7][13] + PS60
nextP[5][7] = PS160*P[7][15] - PS162*P[7][13] - PS60*P[1][7] + PS62*P[2][7] - PS65*P[7][14] + PS72*P[3][7] + PS74*P[0][7] + P[5][7] + dt*(PS160*P[4][15] - PS162*P[4][13] - PS60*P[1][4] + PS62*P[2][4] - PS65*P[4][14] + PS72*P[3][4] + PS74*P[0][4] + P[4][5]);
nextP[6][7] = -PS165*P[7][14] + PS166*P[7][13] + PS60*P[0][7] + PS62*P[3][7] - PS70*P[7][15] - PS72*P[2][7] + PS74*P[1][7] + P[6][7] + dt*(-PS165*P[4][14] + PS166*P[4][13] + PS60*P[0][4] + PS62*P[3][4] - PS70*P[4][15] - PS72*P[2][4] + PS74*P[1][4] + P[4][6]);
nextP[7][7] = P[4][7]*dt + P[7][7] + dt*(P[4][4]*dt + P[4][7]);
nextP[0][8] = -PS11*P[1][8] - PS12*P[2][8] - PS13*P[3][8] + PS6*P[8][10] + PS69*dt + PS7*P[8][11] + PS9*P[8][12] + P[0][8];
nextP[1][8] = PS100*dt + PS11*P[0][8] - PS12*P[3][8] + PS13*P[2][8] - PS34*P[8][10] - PS7*P[8][12] + PS9*P[8][11] + P[1][8];
nextP[2][8] = PS11*P[3][8] + PS12*P[0][8] + PS121*dt - PS13*P[1][8] - PS34*P[8][11] + PS6*P[8][12] - PS9*P[8][10] + P[2][8];
nextP[3][8] = -PS11*P[2][8] + PS12*P[1][8] + PS13*P[0][8] + PS137*dt - PS34*P[8][12] - PS6*P[8][11] + PS7*P[8][10] + P[3][8];
nextP[4][8] = -PS139*P[8][15] + PS140*P[8][14] + PS164*dt - PS44*P[8][13] + PS60*P[2][8] + PS62*P[1][8] + PS72*P[0][8] - PS74*P[3][8] + P[4][8];
nextP[5][8] = PS160*P[8][15] - PS162*P[8][13] + PS181*dt - PS60*P[1][8] + PS62*P[2][8] - PS65*P[8][14] + PS72*P[3][8] + PS74*P[0][8] + P[5][8];
nextP[6][8] = -PS165*P[8][14] + PS166*P[8][13] + PS60*P[0][8] + PS62*P[3][8] - PS70*P[8][15] - PS72*P[2][8] + PS74*P[1][8] + P[6][8] + dt*(-PS165*P[5][14] + PS166*P[5][13] + PS60*P[0][5] + PS62*P[3][5] - PS70*P[5][15] - PS72*P[2][5] + PS74*P[1][5] + P[5][6]);
nextP[0][8] = -PS11*P[1][8] - PS12*P[2][8] - PS13*P[3][8] + PS6*P[8][10] + PS7*P[8][11] + PS86*dt + PS9*P[8][12] + P[0][8];
nextP[1][8] = PS11*P[0][8] - PS12*P[3][8] + PS124*dt + PS13*P[2][8] - PS34*P[8][10] - PS7*P[8][12] + PS9*P[8][11] + P[1][8];
nextP[2][8] = PS11*P[3][8] + PS12*P[0][8] - PS13*P[1][8] + PS149*dt - PS34*P[8][11] + PS6*P[8][12] - PS9*P[8][10] + P[2][8];
nextP[3][8] = -PS11*P[2][8] + PS12*P[1][8] + PS13*P[0][8] + PS169*dt - PS34*P[8][12] - PS6*P[8][11] + PS7*P[8][10] + P[3][8];
nextP[4][8] = -PS171*P[8][15] + PS172*P[8][14] + PS173*P[1][8] + PS174*P[0][8] + PS175*P[2][8] - PS176*P[3][8] + PS196*dt + PS43*P[8][13] + P[4][8];
nextP[5][8] = PS190*P[8][15] - PS193*P[8][13] + PS201*P[2][8] - PS202*P[0][8] + PS203*P[3][8] - PS204*P[1][8] + PS213*dt + PS75*P[8][14] + P[5][8];
nextP[6][8] = -PS197*P[8][14] + PS199*P[8][13] - PS214*P[2][8] + PS215*P[3][8] + PS216*P[0][8] + PS217*P[1][8] + PS87*P[8][15] + P[6][8] + dt*(-PS197*P[5][14] + PS199*P[5][13] - PS214*P[2][5] + PS215*P[3][5] + PS216*P[0][5] + PS217*P[1][5] + PS87*P[5][15] + P[5][6]);
nextP[7][8] = P[4][8]*dt + P[7][8] + dt*(P[4][5]*dt + P[5][7]);
nextP[8][8] = P[5][8]*dt + P[8][8] + dt*(P[5][5]*dt + P[5][8]);
nextP[0][9] = -PS11*P[1][9] - PS12*P[2][9] - PS13*P[3][9] + PS6*P[9][10] + PS7*P[9][11] + PS75*dt + PS9*P[9][12] + P[0][9];
nextP[1][9] = PS101*dt + PS11*P[0][9] - PS12*P[3][9] + PS13*P[2][9] - PS34*P[9][10] - PS7*P[9][12] + PS9*P[9][11] + P[1][9];
nextP[2][9] = PS11*P[3][9] + PS12*P[0][9] + PS122*dt - PS13*P[1][9] - PS34*P[9][11] + PS6*P[9][12] - PS9*P[9][10] + P[2][9];
nextP[3][9] = -PS11*P[2][9] + PS12*P[1][9] + PS13*P[0][9] + PS138*dt - PS34*P[9][12] - PS6*P[9][11] + PS7*P[9][10] + P[3][9];
nextP[4][9] = -PS139*P[9][15] + PS140*P[9][14] + PS168*dt - PS44*P[9][13] + PS60*P[2][9] + PS62*P[1][9] + PS72*P[0][9] - PS74*P[3][9] + P[4][9];
nextP[5][9] = PS160*P[9][15] - PS162*P[9][13] + PS182*dt - PS60*P[1][9] + PS62*P[2][9] - PS65*P[9][14] + PS72*P[3][9] + PS74*P[0][9] + P[5][9];
nextP[6][9] = -PS165*P[9][14] + PS166*P[9][13] + PS186*dt + PS60*P[0][9] + PS62*P[3][9] - PS70*P[9][15] - PS72*P[2][9] + PS74*P[1][9] + P[6][9];
nextP[0][9] = -PS11*P[1][9] - PS12*P[2][9] - PS13*P[3][9] + PS6*P[9][10] + PS7*P[9][11] + PS9*P[9][12] + PS94*dt + P[0][9];
nextP[1][9] = PS11*P[0][9] - PS12*P[3][9] + PS125*dt + PS13*P[2][9] - PS34*P[9][10] - PS7*P[9][12] + PS9*P[9][11] + P[1][9];
nextP[2][9] = PS11*P[3][9] + PS12*P[0][9] - PS13*P[1][9] + PS150*dt - PS34*P[9][11] + PS6*P[9][12] - PS9*P[9][10] + P[2][9];
nextP[3][9] = -PS11*P[2][9] + PS12*P[1][9] + PS13*P[0][9] + PS170*dt - PS34*P[9][12] - PS6*P[9][11] + PS7*P[9][10] + P[3][9];
nextP[4][9] = -PS171*P[9][15] + PS172*P[9][14] + PS173*P[1][9] + PS174*P[0][9] + PS175*P[2][9] - PS176*P[3][9] + PS200*dt + PS43*P[9][13] + P[4][9];
nextP[5][9] = PS190*P[9][15] - PS193*P[9][13] + PS201*P[2][9] - PS202*P[0][9] + PS203*P[3][9] - PS204*P[1][9] + PS218*dt + PS75*P[9][14] + P[5][9];
nextP[6][9] = -PS197*P[9][14] + PS199*P[9][13] - PS214*P[2][9] + PS215*P[3][9] + PS216*P[0][9] + PS217*P[1][9] + PS222*dt + PS87*P[9][15] + P[6][9];
nextP[7][9] = P[4][9]*dt + P[7][9] + dt*(P[4][6]*dt + P[6][7]);
nextP[8][9] = P[5][9]*dt + P[8][9] + dt*(P[5][6]*dt + P[6][8]);
nextP[9][9] = P[6][9]*dt + P[9][9] + dt*(P[6][6]*dt + P[6][9]);
nextP[0][10] = PS14;
nextP[1][10] = PS85;
nextP[2][10] = PS109;
nextP[3][10] = PS123;
nextP[4][10] = -PS139*P[10][15] + PS140*P[10][14] - PS44*P[10][13] + PS60*P[2][10] + PS62*P[1][10] + PS72*P[0][10] - PS74*P[3][10] + P[4][10];
nextP[5][10] = PS160*P[10][15] - PS162*P[10][13] - PS60*P[1][10] + PS62*P[2][10] - PS65*P[10][14] + PS72*P[3][10] + PS74*P[0][10] + P[5][10];
nextP[6][10] = -PS165*P[10][14] + PS166*P[10][13] + PS60*P[0][10] + PS62*P[3][10] - PS70*P[10][15] - PS72*P[2][10] + PS74*P[1][10] + P[6][10];
nextP[1][10] = PS105;
nextP[2][10] = PS133;
nextP[3][10] = PS151;
nextP[4][10] = -PS171*P[10][15] + PS172*P[10][14] + PS173*P[1][10] + PS174*P[0][10] + PS175*P[2][10] - PS176*P[3][10] + PS43*P[10][13] + P[4][10];
nextP[5][10] = PS190*P[10][15] - PS193*P[10][13] + PS201*P[2][10] - PS202*P[0][10] + PS203*P[3][10] - PS204*P[1][10] + PS75*P[10][14] + P[5][10];
nextP[6][10] = -PS197*P[10][14] + PS199*P[10][13] - PS214*P[2][10] + PS215*P[3][10] + PS216*P[0][10] + PS217*P[1][10] + PS87*P[10][15] + P[6][10];
nextP[7][10] = P[4][10]*dt + P[7][10];
nextP[8][10] = P[5][10]*dt + P[8][10];
nextP[9][10] = P[6][10]*dt + P[9][10];
nextP[10][10] = P[10][10];
nextP[0][11] = PS17;
nextP[1][11] = PS77;
nextP[2][11] = PS108;
nextP[3][11] = PS127;
nextP[4][11] = -PS139*P[11][15] + PS140*P[11][14] - PS44*P[11][13] + PS60*P[2][11] + PS62*P[1][11] + PS72*P[0][11] - PS74*P[3][11] + P[4][11];
nextP[5][11] = PS160*P[11][15] - PS162*P[11][13] - PS60*P[1][11] + PS62*P[2][11] - PS65*P[11][14] + PS72*P[3][11] + PS74*P[0][11] + P[5][11];
nextP[6][11] = -PS165*P[11][14] + PS166*P[11][13] + PS60*P[0][11] + PS62*P[3][11] - PS70*P[11][15] - PS72*P[2][11] + PS74*P[1][11] + P[6][11];
nextP[1][11] = PS97;
nextP[2][11] = PS132;
nextP[3][11] = PS155;
nextP[4][11] = -PS171*P[11][15] + PS172*P[11][14] + PS173*P[1][11] + PS174*P[0][11] + PS175*P[2][11] - PS176*P[3][11] + PS43*P[11][13] + P[4][11];
nextP[5][11] = PS190*P[11][15] - PS193*P[11][13] + PS201*P[2][11] - PS202*P[0][11] + PS203*P[3][11] - PS204*P[1][11] + PS75*P[11][14] + P[5][11];
nextP[6][11] = -PS197*P[11][14] + PS199*P[11][13] - PS214*P[2][11] + PS215*P[3][11] + PS216*P[0][11] + PS217*P[1][11] + PS87*P[11][15] + P[6][11];
nextP[7][11] = P[4][11]*dt + P[7][11];
nextP[8][11] = P[5][11]*dt + P[8][11];
nextP[9][11] = P[6][11]*dt + P[9][11];
nextP[10][11] = P[10][11];
nextP[11][11] = P[11][11];
nextP[0][12] = PS20;
nextP[1][12] = PS87;
nextP[2][12] = PS103;
nextP[3][12] = PS126;
nextP[4][12] = -PS139*P[12][15] + PS140*P[12][14] - PS44*P[12][13] + PS60*P[2][12] + PS62*P[1][12] + PS72*P[0][12] - PS74*P[3][12] + P[4][12];
nextP[5][12] = PS160*P[12][15] - PS162*P[12][13] - PS60*P[1][12] + PS62*P[2][12] - PS65*P[12][14] + PS72*P[3][12] + PS74*P[0][12] + P[5][12];
nextP[6][12] = -PS165*P[12][14] + PS166*P[12][13] + PS60*P[0][12] + PS62*P[3][12] - PS70*P[12][15] - PS72*P[2][12] + PS74*P[1][12] + P[6][12];
nextP[1][12] = PS107;
nextP[2][12] = PS127;
nextP[3][12] = PS154;
nextP[4][12] = -PS171*P[12][15] + PS172*P[12][14] + PS173*P[1][12] + PS174*P[0][12] + PS175*P[2][12] - PS176*P[3][12] + PS43*P[12][13] + P[4][12];
nextP[5][12] = PS190*P[12][15] - PS193*P[12][13] + PS201*P[2][12] - PS202*P[0][12] + PS203*P[3][12] - PS204*P[1][12] + PS75*P[12][14] + P[5][12];
nextP[6][12] = -PS197*P[12][14] + PS199*P[12][13] - PS214*P[2][12] + PS215*P[3][12] + PS216*P[0][12] + PS217*P[1][12] + PS87*P[12][15] + P[6][12];
nextP[7][12] = P[4][12]*dt + P[7][12];
nextP[8][12] = P[5][12]*dt + P[8][12];
nextP[9][12] = P[6][12]*dt + P[9][12];
nextP[10][12] = P[10][12];
nextP[11][12] = P[11][12];
nextP[12][12] = P[12][12];
nextP[0][13] = PS45;
nextP[1][13] = PS93;
nextP[2][13] = PS114;
nextP[3][13] = PS130;
nextP[4][13] = PS141;
nextP[5][13] = PS170;
nextP[6][13] = PS185;
nextP[0][13] = PS44;
nextP[1][13] = PS113;
nextP[2][13] = PS138;
nextP[3][13] = PS158;
nextP[4][13] = PS177;
nextP[5][13] = PS206;
nextP[6][13] = PS221;
nextP[7][13] = P[4][13]*dt + P[7][13];
nextP[8][13] = P[5][13]*dt + P[8][13];
nextP[9][13] = P[6][13]*dt + P[9][13];
@ -293,13 +293,13 @@ nextP[10][13] = P[10][13];
nextP[11][13] = P[11][13];
nextP[12][13] = P[12][13];
nextP[13][13] = P[13][13];
nextP[0][14] = PS55;
nextP[1][14] = PS96;
nextP[2][14] = PS117;
nextP[3][14] = PS133;
nextP[4][14] = PS146;
nextP[5][14] = PS169;
nextP[6][14] = PS184;
nextP[0][14] = PS57;
nextP[1][14] = PS117;
nextP[2][14] = PS142;
nextP[3][14] = PS162;
nextP[4][14] = PS180;
nextP[5][14] = PS205;
nextP[6][14] = PS220;
nextP[7][14] = P[4][14]*dt + P[7][14];
nextP[8][14] = P[5][14]*dt + P[8][14];
nextP[9][14] = P[6][14]*dt + P[9][14];
@ -308,13 +308,13 @@ 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[0][15] = PS47;
nextP[1][15] = PS94;
nextP[2][15] = PS115;
nextP[3][15] = PS131;
nextP[4][15] = PS142;
nextP[5][15] = PS173;
nextP[6][15] = PS183;
nextP[0][15] = PS46;
nextP[1][15] = PS114;
nextP[2][15] = PS139;
nextP[3][15] = PS159;
nextP[4][15] = PS178;
nextP[5][15] = PS209;
nextP[6][15] = PS219;
nextP[7][15] = P[4][15]*dt + P[7][15];
nextP[8][15] = P[5][15]*dt + P[8][15];
nextP[9][15] = P[6][15]*dt + P[9][15];
@ -328,9 +328,9 @@ nextP[0][16] = -PS11*P[1][16] - PS12*P[2][16] - PS13*P[3][16] + PS6*P[10][16] +
nextP[1][16] = PS11*P[0][16] - PS12*P[3][16] + PS13*P[2][16] - PS34*P[10][16] - PS7*P[12][16] + PS9*P[11][16] + P[1][16];
nextP[2][16] = PS11*P[3][16] + PS12*P[0][16] - PS13*P[1][16] - PS34*P[11][16] + PS6*P[12][16] - PS9*P[10][16] + P[2][16];
nextP[3][16] = -PS11*P[2][16] + PS12*P[1][16] + PS13*P[0][16] - PS34*P[12][16] - PS6*P[11][16] + PS7*P[10][16] + P[3][16];
nextP[4][16] = -PS139*P[15][16] + PS140*P[14][16] - PS44*P[13][16] + PS60*P[2][16] + PS62*P[1][16] + PS72*P[0][16] - PS74*P[3][16] + P[4][16];
nextP[5][16] = PS160*P[15][16] - PS162*P[13][16] - PS60*P[1][16] + PS62*P[2][16] - PS65*P[14][16] + PS72*P[3][16] + PS74*P[0][16] + P[5][16];
nextP[6][16] = -PS165*P[14][16] + PS166*P[13][16] + PS60*P[0][16] + PS62*P[3][16] - PS70*P[15][16] - PS72*P[2][16] + PS74*P[1][16] + P[6][16];
nextP[4][16] = -PS171*P[15][16] + PS172*P[14][16] + PS173*P[1][16] + PS174*P[0][16] + PS175*P[2][16] - PS176*P[3][16] + PS43*P[13][16] + P[4][16];
nextP[5][16] = PS190*P[15][16] - PS193*P[13][16] + PS201*P[2][16] - PS202*P[0][16] + PS203*P[3][16] - PS204*P[1][16] + PS75*P[14][16] + P[5][16];
nextP[6][16] = -PS197*P[14][16] + PS199*P[13][16] - PS214*P[2][16] + PS215*P[3][16] + PS216*P[0][16] + PS217*P[1][16] + PS87*P[15][16] + P[6][16];
nextP[7][16] = P[4][16]*dt + P[7][16];
nextP[8][16] = P[5][16]*dt + P[8][16];
nextP[9][16] = P[6][16]*dt + P[9][16];
@ -345,9 +345,9 @@ nextP[0][17] = -PS11*P[1][17] - PS12*P[2][17] - PS13*P[3][17] + PS6*P[10][17] +
nextP[1][17] = PS11*P[0][17] - PS12*P[3][17] + PS13*P[2][17] - PS34*P[10][17] - PS7*P[12][17] + PS9*P[11][17] + P[1][17];
nextP[2][17] = PS11*P[3][17] + PS12*P[0][17] - PS13*P[1][17] - PS34*P[11][17] + PS6*P[12][17] - PS9*P[10][17] + P[2][17];
nextP[3][17] = -PS11*P[2][17] + PS12*P[1][17] + PS13*P[0][17] - PS34*P[12][17] - PS6*P[11][17] + PS7*P[10][17] + P[3][17];
nextP[4][17] = -PS139*P[15][17] + PS140*P[14][17] - PS44*P[13][17] + PS60*P[2][17] + PS62*P[1][17] + PS72*P[0][17] - PS74*P[3][17] + P[4][17];
nextP[5][17] = PS160*P[15][17] - PS162*P[13][17] - PS60*P[1][17] + PS62*P[2][17] - PS65*P[14][17] + PS72*P[3][17] + PS74*P[0][17] + P[5][17];
nextP[6][17] = -PS165*P[14][17] + PS166*P[13][17] + PS60*P[0][17] + PS62*P[3][17] - PS70*P[15][17] - PS72*P[2][17] + PS74*P[1][17] + P[6][17];
nextP[4][17] = -PS171*P[15][17] + PS172*P[14][17] + PS173*P[1][17] + PS174*P[0][17] + PS175*P[2][17] - PS176*P[3][17] + PS43*P[13][17] + P[4][17];
nextP[5][17] = PS190*P[15][17] - PS193*P[13][17] + PS201*P[2][17] - PS202*P[0][17] + PS203*P[3][17] - PS204*P[1][17] + PS75*P[14][17] + P[5][17];
nextP[6][17] = -PS197*P[14][17] + PS199*P[13][17] - PS214*P[2][17] + PS215*P[3][17] + PS216*P[0][17] + PS217*P[1][17] + PS87*P[15][17] + P[6][17];
nextP[7][17] = P[4][17]*dt + P[7][17];
nextP[8][17] = P[5][17]*dt + P[8][17];
nextP[9][17] = P[6][17]*dt + P[9][17];
@ -363,9 +363,9 @@ nextP[0][18] = -PS11*P[1][18] - PS12*P[2][18] - PS13*P[3][18] + PS6*P[10][18] +
nextP[1][18] = PS11*P[0][18] - PS12*P[3][18] + PS13*P[2][18] - PS34*P[10][18] - PS7*P[12][18] + PS9*P[11][18] + P[1][18];
nextP[2][18] = PS11*P[3][18] + PS12*P[0][18] - PS13*P[1][18] - PS34*P[11][18] + PS6*P[12][18] - PS9*P[10][18] + P[2][18];
nextP[3][18] = -PS11*P[2][18] + PS12*P[1][18] + PS13*P[0][18] - PS34*P[12][18] - PS6*P[11][18] + PS7*P[10][18] + P[3][18];
nextP[4][18] = -PS139*P[15][18] + PS140*P[14][18] - PS44*P[13][18] + PS60*P[2][18] + PS62*P[1][18] + PS72*P[0][18] - PS74*P[3][18] + P[4][18];
nextP[5][18] = PS160*P[15][18] - PS162*P[13][18] - PS60*P[1][18] + PS62*P[2][18] - PS65*P[14][18] + PS72*P[3][18] + PS74*P[0][18] + P[5][18];
nextP[6][18] = -PS165*P[14][18] + PS166*P[13][18] + PS60*P[0][18] + PS62*P[3][18] - PS70*P[15][18] - PS72*P[2][18] + PS74*P[1][18] + P[6][18];
nextP[4][18] = -PS171*P[15][18] + PS172*P[14][18] + PS173*P[1][18] + PS174*P[0][18] + PS175*P[2][18] - PS176*P[3][18] + PS43*P[13][18] + P[4][18];
nextP[5][18] = PS190*P[15][18] - PS193*P[13][18] + PS201*P[2][18] - PS202*P[0][18] + PS203*P[3][18] - PS204*P[1][18] + PS75*P[14][18] + P[5][18];
nextP[6][18] = -PS197*P[14][18] + PS199*P[13][18] - PS214*P[2][18] + PS215*P[3][18] + PS216*P[0][18] + PS217*P[1][18] + PS87*P[15][18] + P[6][18];
nextP[7][18] = P[4][18]*dt + P[7][18];
nextP[8][18] = P[5][18]*dt + P[8][18];
nextP[9][18] = P[6][18]*dt + P[9][18];
@ -382,9 +382,9 @@ nextP[0][19] = -PS11*P[1][19] - PS12*P[2][19] - PS13*P[3][19] + PS6*P[10][19] +
nextP[1][19] = PS11*P[0][19] - PS12*P[3][19] + PS13*P[2][19] - PS34*P[10][19] - PS7*P[12][19] + PS9*P[11][19] + P[1][19];
nextP[2][19] = PS11*P[3][19] + PS12*P[0][19] - PS13*P[1][19] - PS34*P[11][19] + PS6*P[12][19] - PS9*P[10][19] + P[2][19];
nextP[3][19] = -PS11*P[2][19] + PS12*P[1][19] + PS13*P[0][19] - PS34*P[12][19] - PS6*P[11][19] + PS7*P[10][19] + P[3][19];
nextP[4][19] = -PS139*P[15][19] + PS140*P[14][19] - PS44*P[13][19] + PS60*P[2][19] + PS62*P[1][19] + PS72*P[0][19] - PS74*P[3][19] + P[4][19];
nextP[5][19] = PS160*P[15][19] - PS162*P[13][19] - PS60*P[1][19] + PS62*P[2][19] - PS65*P[14][19] + PS72*P[3][19] + PS74*P[0][19] + P[5][19];
nextP[6][19] = -PS165*P[14][19] + PS166*P[13][19] + PS60*P[0][19] + PS62*P[3][19] - PS70*P[15][19] - PS72*P[2][19] + PS74*P[1][19] + P[6][19];
nextP[4][19] = -PS171*P[15][19] + PS172*P[14][19] + PS173*P[1][19] + PS174*P[0][19] + PS175*P[2][19] - PS176*P[3][19] + PS43*P[13][19] + P[4][19];
nextP[5][19] = PS190*P[15][19] - PS193*P[13][19] + PS201*P[2][19] - PS202*P[0][19] + PS203*P[3][19] - PS204*P[1][19] + PS75*P[14][19] + P[5][19];
nextP[6][19] = -PS197*P[14][19] + PS199*P[13][19] - PS214*P[2][19] + PS215*P[3][19] + PS216*P[0][19] + PS217*P[1][19] + PS87*P[15][19] + P[6][19];
nextP[7][19] = P[4][19]*dt + P[7][19];
nextP[8][19] = P[5][19]*dt + P[8][19];
nextP[9][19] = P[6][19]*dt + P[9][19];
@ -402,9 +402,9 @@ nextP[0][20] = -PS11*P[1][20] - PS12*P[2][20] - PS13*P[3][20] + PS6*P[10][20] +
nextP[1][20] = PS11*P[0][20] - PS12*P[3][20] + PS13*P[2][20] - PS34*P[10][20] - PS7*P[12][20] + PS9*P[11][20] + P[1][20];
nextP[2][20] = PS11*P[3][20] + PS12*P[0][20] - PS13*P[1][20] - PS34*P[11][20] + PS6*P[12][20] - PS9*P[10][20] + P[2][20];
nextP[3][20] = -PS11*P[2][20] + PS12*P[1][20] + PS13*P[0][20] - PS34*P[12][20] - PS6*P[11][20] + PS7*P[10][20] + P[3][20];
nextP[4][20] = -PS139*P[15][20] + PS140*P[14][20] - PS44*P[13][20] + PS60*P[2][20] + PS62*P[1][20] + PS72*P[0][20] - PS74*P[3][20] + P[4][20];
nextP[5][20] = PS160*P[15][20] - PS162*P[13][20] - PS60*P[1][20] + PS62*P[2][20] - PS65*P[14][20] + PS72*P[3][20] + PS74*P[0][20] + P[5][20];
nextP[6][20] = -PS165*P[14][20] + PS166*P[13][20] + PS60*P[0][20] + PS62*P[3][20] - PS70*P[15][20] - PS72*P[2][20] + PS74*P[1][20] + P[6][20];
nextP[4][20] = -PS171*P[15][20] + PS172*P[14][20] + PS173*P[1][20] + PS174*P[0][20] + PS175*P[2][20] - PS176*P[3][20] + PS43*P[13][20] + P[4][20];
nextP[5][20] = PS190*P[15][20] - PS193*P[13][20] + PS201*P[2][20] - PS202*P[0][20] + PS203*P[3][20] - PS204*P[1][20] + PS75*P[14][20] + P[5][20];
nextP[6][20] = -PS197*P[14][20] + PS199*P[13][20] - PS214*P[2][20] + PS215*P[3][20] + PS216*P[0][20] + PS217*P[1][20] + PS87*P[15][20] + P[6][20];
nextP[7][20] = P[4][20]*dt + P[7][20];
nextP[8][20] = P[5][20]*dt + P[8][20];
nextP[9][20] = P[6][20]*dt + P[9][20];
@ -423,9 +423,9 @@ nextP[0][21] = -PS11*P[1][21] - PS12*P[2][21] - PS13*P[3][21] + PS6*P[10][21] +
nextP[1][21] = PS11*P[0][21] - PS12*P[3][21] + PS13*P[2][21] - PS34*P[10][21] - PS7*P[12][21] + PS9*P[11][21] + P[1][21];
nextP[2][21] = PS11*P[3][21] + PS12*P[0][21] - PS13*P[1][21] - PS34*P[11][21] + PS6*P[12][21] - PS9*P[10][21] + P[2][21];
nextP[3][21] = -PS11*P[2][21] + PS12*P[1][21] + PS13*P[0][21] - PS34*P[12][21] - PS6*P[11][21] + PS7*P[10][21] + P[3][21];
nextP[4][21] = -PS139*P[15][21] + PS140*P[14][21] - PS44*P[13][21] + PS60*P[2][21] + PS62*P[1][21] + PS72*P[0][21] - PS74*P[3][21] + P[4][21];
nextP[5][21] = PS160*P[15][21] - PS162*P[13][21] - PS60*P[1][21] + PS62*P[2][21] - PS65*P[14][21] + PS72*P[3][21] + PS74*P[0][21] + P[5][21];
nextP[6][21] = -PS165*P[14][21] + PS166*P[13][21] + PS60*P[0][21] + PS62*P[3][21] - PS70*P[15][21] - PS72*P[2][21] + PS74*P[1][21] + P[6][21];
nextP[4][21] = -PS171*P[15][21] + PS172*P[14][21] + PS173*P[1][21] + PS174*P[0][21] + PS175*P[2][21] - PS176*P[3][21] + PS43*P[13][21] + P[4][21];
nextP[5][21] = PS190*P[15][21] - PS193*P[13][21] + PS201*P[2][21] - PS202*P[0][21] + PS203*P[3][21] - PS204*P[1][21] + PS75*P[14][21] + P[5][21];
nextP[6][21] = -PS197*P[14][21] + PS199*P[13][21] - PS214*P[2][21] + PS215*P[3][21] + PS216*P[0][21] + PS217*P[1][21] + PS87*P[15][21] + P[6][21];
nextP[7][21] = P[4][21]*dt + P[7][21];
nextP[8][21] = P[5][21]*dt + P[8][21];
nextP[9][21] = P[6][21]*dt + P[9][21];
@ -445,9 +445,9 @@ nextP[0][22] = -PS11*P[1][22] - PS12*P[2][22] - PS13*P[3][22] + PS6*P[10][22] +
nextP[1][22] = PS11*P[0][22] - PS12*P[3][22] + PS13*P[2][22] - PS34*P[10][22] - PS7*P[12][22] + PS9*P[11][22] + P[1][22];
nextP[2][22] = PS11*P[3][22] + PS12*P[0][22] - PS13*P[1][22] - PS34*P[11][22] + PS6*P[12][22] - PS9*P[10][22] + P[2][22];
nextP[3][22] = -PS11*P[2][22] + PS12*P[1][22] + PS13*P[0][22] - PS34*P[12][22] - PS6*P[11][22] + PS7*P[10][22] + P[3][22];
nextP[4][22] = -PS139*P[15][22] + PS140*P[14][22] - PS44*P[13][22] + PS60*P[2][22] + PS62*P[1][22] + PS72*P[0][22] - PS74*P[3][22] + P[4][22];
nextP[5][22] = PS160*P[15][22] - PS162*P[13][22] - PS60*P[1][22] + PS62*P[2][22] - PS65*P[14][22] + PS72*P[3][22] + PS74*P[0][22] + P[5][22];
nextP[6][22] = -PS165*P[14][22] + PS166*P[13][22] + PS60*P[0][22] + PS62*P[3][22] - PS70*P[15][22] - PS72*P[2][22] + PS74*P[1][22] + P[6][22];
nextP[4][22] = -PS171*P[15][22] + PS172*P[14][22] + PS173*P[1][22] + PS174*P[0][22] + PS175*P[2][22] - PS176*P[3][22] + PS43*P[13][22] + P[4][22];
nextP[5][22] = PS190*P[15][22] - PS193*P[13][22] + PS201*P[2][22] - PS202*P[0][22] + PS203*P[3][22] - PS204*P[1][22] + PS75*P[14][22] + P[5][22];
nextP[6][22] = -PS197*P[14][22] + PS199*P[13][22] - PS214*P[2][22] + PS215*P[3][22] + PS216*P[0][22] + PS217*P[1][22] + PS87*P[15][22] + P[6][22];
nextP[7][22] = P[4][22]*dt + P[7][22];
nextP[8][22] = P[5][22]*dt + P[8][22];
nextP[9][22] = P[6][22]*dt + P[9][22];
@ -468,9 +468,9 @@ nextP[0][23] = -PS11*P[1][23] - PS12*P[2][23] - PS13*P[3][23] + PS6*P[10][23] +
nextP[1][23] = PS11*P[0][23] - PS12*P[3][23] + PS13*P[2][23] - PS34*P[10][23] - PS7*P[12][23] + PS9*P[11][23] + P[1][23];
nextP[2][23] = PS11*P[3][23] + PS12*P[0][23] - PS13*P[1][23] - PS34*P[11][23] + PS6*P[12][23] - PS9*P[10][23] + P[2][23];
nextP[3][23] = -PS11*P[2][23] + PS12*P[1][23] + PS13*P[0][23] - PS34*P[12][23] - PS6*P[11][23] + PS7*P[10][23] + P[3][23];
nextP[4][23] = -PS139*P[15][23] + PS140*P[14][23] - PS44*P[13][23] + PS60*P[2][23] + PS62*P[1][23] + PS72*P[0][23] - PS74*P[3][23] + P[4][23];
nextP[5][23] = PS160*P[15][23] - PS162*P[13][23] - PS60*P[1][23] + PS62*P[2][23] - PS65*P[14][23] + PS72*P[3][23] + PS74*P[0][23] + P[5][23];
nextP[6][23] = -PS165*P[14][23] + PS166*P[13][23] + PS60*P[0][23] + PS62*P[3][23] - PS70*P[15][23] - PS72*P[2][23] + PS74*P[1][23] + P[6][23];
nextP[4][23] = -PS171*P[15][23] + PS172*P[14][23] + PS173*P[1][23] + PS174*P[0][23] + PS175*P[2][23] - PS176*P[3][23] + PS43*P[13][23] + P[4][23];
nextP[5][23] = PS190*P[15][23] - PS193*P[13][23] + PS201*P[2][23] - PS202*P[0][23] + PS203*P[3][23] - PS204*P[1][23] + PS75*P[14][23] + P[5][23];
nextP[6][23] = -PS197*P[14][23] + PS199*P[13][23] - PS214*P[2][23] + PS215*P[3][23] + PS216*P[0][23] + PS217*P[1][23] + PS87*P[15][23] + P[6][23];
nextP[7][23] = P[4][23]*dt + P[7][23];
nextP[8][23] = P[5][23]*dt + P[8][23];
nextP[9][23] = P[6][23]*dt + P[9][23];

20
libraries/AP_NavEKF3/derivation/main.py
View File

@ -13,9 +13,23 @@ def quat2Rot(q):
q2 = q[2]
q3 = q[3]
Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
# This form is the one normally used in flight dynamics and inertial navigation texts, eg
# Aircraft Control and Simulation, Stevens,B.L, Lewis,F.L, Johnson,E.N, Third Edition, eqn 1.8-18
# It does produce second order terms in the covariance prediction that can be problematic
# with single precision processing.
# It requires the quternion to be unit length.
# Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
# [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
# [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
# This form removes q1 from the 0,0, q2 from the 1,1 and q3 from the 2,2 entry and results
# in a covariance prediction that is better conditioned.
# It requires the quaternion to be unit length and is mathematically identical
# to the alternate form when q0**2 + q1**2 + q2**2 + q3**2 = 1
# See https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
Rot = Matrix([[1 - 2*(q2**2 + q3**2), 2*(q1*q2 - q0*q3) , 2*(q1*q3 + q0*q2) ],
[2*(q1*q2 + q0*q3) , 1 - 2*(q1**2 + q3**2), 2*(q2*q3 - q0*q1) ],
[2*(q1*q3-q0*q2) , 2*(q2*q3 + q0*q1) , 1 - 2*(q1**2 + q2**2)]])
return Rot