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

AP_NavEKF3: Remove commented code

This commit is contained in:
Paul Riseborough 2020-12-09 20:00:29 +11:00 committed by Andrew Tridgell
parent b86c014400
commit 171f0b399f
1 changed files with 2 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -580,28 +580,7 @@ void NavEKF3_core::FuseDragForces()
// Kalman gains
// Don't allow modification of any states other than wind velocity - we only need a wind estimate.
// Kfusion[0] = -HK18*HK32;
// Kfusion[1] = -HK29*HK32;
// Kfusion[2] = -HK30*HK32;
// Kfusion[3] = -HK31*HK32;
// Kfusion[4] = -HK26*HK32;
// Kfusion[5] = -HK23*HK32;
// Kfusion[6] = -HK24*HK32;
// Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]);
// Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]);
// Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]);
// Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]);
// Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]);
// Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]);
// Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]);
// Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]);
// Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]);
// Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]);
// Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]);
// Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]);
// Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]);
// Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]);
// Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]);
// See AP_NavEKF3/derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations.
Kfusion[22] = -HK28*HK32;
Kfusion[23] = -HK20*HK32;
@ -684,28 +663,7 @@ void NavEKF3_core::FuseDragForces()
// Kalman gains
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
// Kfusion[0] = -HK18*HK32;
// Kfusion[1] = -HK30*HK32;
// Kfusion[2] = -HK29*HK32;
// Kfusion[3] = -HK31*HK32;
// Kfusion[4] = -HK24*HK32;
// Kfusion[5] = -HK26*HK32;
// Kfusion[6] = -HK20*HK32;
// Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]);
// Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]);
// Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]);
// Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]);
// Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]);
// Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]);
// Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]);
// Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]);
// Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]);
// Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]);
// Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]);
// Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]);
// Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]);
// Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]);
// Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]);
// See AP_NavEKF3/derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations.
Kfusion[22] = -HK22*HK32;
Kfusion[23] = -HK28*HK32;
}