rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

OP-1750 - Performance optimization for Ins13 lib

Browse Source
This commit is contained in:
Alessio Morale 2015-02-26 06:41:19 +01:00
parent e2c9111b39
commit 362c0c67b7
1 changed files with 29 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/libraries/insgps13state.c
View File

@ -40,7 +40,7 @@
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector #define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector #define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector #define NUMU 6 // number of deterministic inputs, U is the input vector
#pragma GCC optimize "O3"
// Private functions // Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]); float Q[NUMW], float dT, float P[NUMX][NUMX]);
@ -76,14 +76,14 @@ static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// b............. ......oXo // b............. ......oXo
// c............. ......ooX // c............. ......ooX
static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 }; static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 }; static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 }; static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 }; static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 }; static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 }; static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
static struct EKFData { static struct EKFData {
// linearized system matrices // linearized system matrices
@ -183,8 +183,8 @@ void INSGetP(float PDiag[NUMX])
uint8_t i; uint8_t i;
// retrieve diagonal elements (aka state variance) // retrieve diagonal elements (aka state variance)
for (i = 0; i < NUMX; i++) { if (PDiag != 0) {
if (PDiag != 0) { for (i = 0; i < NUMX; i++) {
PDiag[i] = ekf.P[i][i]; PDiag[i] = ekf.P[i][i];
} }
} }
@ -432,7 +432,6 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
// The first Method is very specific to this implementation // The first Method is very specific to this implementation
// ************************************************ // ************************************************
__attribute__((optimize("O3")))
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]) float Q[NUMW], float dT, float P[NUMX][NUMX])
{ {
@ -444,6 +443,7 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Dummy[NUMX][NUMX]; float Dummy[NUMX][NUMX];
int8_t i; int8_t i;
int8_t k; int8_t k;
for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P) for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
float *Firow = F[i]; float *Firow = F[i];
float *Pirow = P[i]; float *Pirow = P[i];
@ -471,11 +471,11 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ... float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
const float *Fjrow = F[j]; const float *Fjrow = F[j];
int8_t Fjstart = FrowMin[j]; int8_t Fjstart = FrowMin[j];
int8_t Fjend = FrowMax[j]; int8_t Fjend = FrowMax[j];
k = Fjstart; k = Fjstart;
while (k <= Fjend - 3) { while (k <= Fjend - 3) {
@ -483,7 +483,7 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
Ptmp += Dirow[k + 1] * Fjrow[k + 1]; Ptmp += Dirow[k + 1] * Fjrow[k + 1];
Ptmp += Dirow[k + 2] * Fjrow[k + 2]; Ptmp += Dirow[k + 2] * Fjrow[k + 2];
Ptmp += Dirow[k + 3] * Fjrow[k + 3]; Ptmp += Dirow[k + 3] * Fjrow[k + 3];
k+=4; k += 4;
} }
while (k <= Fjend) { while (k <= Fjend) {
Ptmp += Dirow[k] * Fjrow[k]; Ptmp += Dirow[k] * Fjrow[k];
@ -492,20 +492,20 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float *Gjrow = G[j]; float *Gjrow = G[j];
const int8_t Gjstart = MAX(Gistart, GrowMin[j]); const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
const int8_t Gjend = MIN(Giend, GrowMax[j]); const int8_t Gjend = MIN(Giend, GrowMax[j]);
k = Gjstart; k = Gjstart;
while (k <= Gjend - 2) { while (k <= Gjend - 2) {
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ... Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2]; Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
k+=3; k += 3;
} }
if (k <= Gjend) { if (k <= Gjend) {
Ptmp += Q[k] * Girow[k] * Gjrow[k]; Ptmp += Q[k] * Girow[k] * Gjrow[k];
if (k <= Gjend - 1) { if (k <= Gjend - 1) {
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1]; Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
} }
} }
P[j][i] = Pirow[j] = Ptmp * dTsq; // [] * (T^2) P[j][i] = Pirow[j] = Ptmp * dTsq; // [] * (T^2)
} }
@ -527,7 +527,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
// The SensorsUsed variable is a bitwise mask indicating which sensors // The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update. // should be used in the update.
// ************************************************ // ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed) uint16_t SensorsUsed)
@ -540,7 +539,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
if (SensorsUsed & (0x01 << m)) { // use this sensor for update if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0; HP[j] = 0;
for (k = HrowMin[m]; k <= HrowMax[m]; k++) { }
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] += H[m][k] * P[k][j]; HP[j] += H[m][k] * P[k][j];
} }
} }
@ -548,14 +550,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
for (k = HrowMin[m]; k <= HrowMax[m]; k++) { for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
HPHR += HP[k] * H[m][k]; HPHR += HP[k] * H[m][k];
} }
float invHPHR = 1.0f / HPHR;
for (k = 0; k < NUMX; k++) { for (k = 0; k < NUMX; k++) {
Km[k] = HP[k] / HPHR; // find K = HP/HPHR Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
} }
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++) { for (j = i; j < NUMX; j++) {
P[i][j] = P[j][i] = P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
P[i][j] - Km[i] * HP[j];
} }
} }
@ -601,7 +602,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
for (i = 0; i < NUMX; i++) { for (i = 0; i < NUMX; i++) {
X[i] = X[i] =
Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] + Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
K4[i]) / 6.0f; K4[i]) * (1.0f / 6.0f);
} }
} }