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

performance optimizations

Browse Source
This commit is contained in:
Alessio Morale 2015-02-24 19:17:53 +01:00
parent a01cab1e72
commit e2c9111b39
1 changed files with 96 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

176
flight/libraries/insgps13state.c
View File

@ -33,6 +33,7 @@
#include <math.h> #include <math.h>
#include <stdint.h> #include <stdint.h>
#include <pios_math.h> #include <pios_math.h>
#include <mathmisc.h>
// constants/macros/typdefs // constants/macros/typdefs
#define NUMX 13 // number of states, X is the state vector #define NUMX 13 // number of states, X is the state vector
@ -43,15 +44,15 @@
// 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]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], static 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);
void RungeKutta(float X[NUMX], float U[NUMU], float dT); static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]); float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables // Private variables
@ -60,22 +61,22 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// LinearizeFG() and LinearizeH(): // LinearizeFG() and LinearizeH():
// //
// usage F: usage G: usage H: // usage F: usage G: usage H:
// 0123456789abc 012345678 0123456789abc // -0123456789abc 012345678 0123456789abc
// 0...X......... ......... X............ // 0...X......... ......... X............
// 1....X........ ......... .X........... // 1....X........ ......... .X...........
// 2.....X....... ......... ..X.......... // 2.....X....... ......... ..X..........
// 3......XXXX... ...XXX... ...X......... // 3......XXXX... ...XXX... ...X.........
// 4......XXXX... ...XXX... ....X........ // 4......XXXX... ...XXX... ....X........
// 5......XXXX... ...XXX... .....X....... // 5......XXXX... ...XXX... .....X.......
// 6.......XXXXXX XXX...... ......XXXX... // 6.....ooXXXXXX XXX...... ......XXXX...
// 7......X.XXXXX XXX...... ......XXXX... // 7.....oXoXXXXX XXX...... ......XXXX...
// 8......XX.XXXX XXX...... ......XXXX... // 8.....oXXoXXXX XXX...... ......XXXX...
// 9......XXX.XXX XXX...... ..X.......... // 9.....oXXXoXXX XXX...... ..X..........
// a............. ......X.. // a............. ......Xoo
// b............. .......X. // b............. ......oXo
// c............. ........X // c............. ......ooX
static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 }; static const 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 const 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 const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3])
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{ {
float U[6]; float U[6];
float qmag; float invqmag;
// rate gyro inputs in units of rad/s // rate gyro inputs in units of rad/s
U[0] = gyro_data[0]; U[0] = gyro_data[0];
@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
// EKF prediction step // EKF prediction step
LinearizeFG(ekf.X, U, ekf.F, ekf.G); LinearizeFG(ekf.X, U, ekf.F, ekf.G);
RungeKutta(ekf.X, U, dT); RungeKutta(ekf.X, U, dT);
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] /= qmag; ekf.X[6] *= invqmag;
ekf.X[7] /= qmag; ekf.X[7] *= invqmag;
ekf.X[8] /= qmag; ekf.X[8] *= invqmag;
ekf.X[9] /= qmag; ekf.X[9] *= invqmag;
// CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P); // CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
// Update Nav solution structure // Update Nav solution structure
@ -373,40 +374,37 @@ void VelBaroCorrection(float Vel[3], float BaroAlt)
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed) float BaroAlt, uint16_t SensorsUsed)
{ {
float Z[10], Y[10]; float Z[10] = { 0 };
float Bmag, qmag; float Y[10] = { 0 };
float invBmag;
// GPS Position in meters and in local NED frame // GPS Position in meters and in local NED frame
Z[0] = Pos[0]; Z[0] = Pos[0];
Z[1] = Pos[1]; Z[1] = Pos[1];
Z[2] = Pos[2]; Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame // GPS Velocity in meters and in local NED frame
Z[3] = Vel[0]; Z[3] = Vel[0];
Z[4] = Vel[1]; Z[4] = Vel[1];
Z[5] = Vel[2]; Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame // magnetometer data in any units (use unit vector) and in body frame
Bmag = invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + Z[6] = mag_data[0] * invBmag;
mag_data[2] * mag_data[2]); Z[7] = mag_data[1] * invBmag;
Z[6] = mag_data[0] / Bmag; Z[8] = mag_data[2] * invBmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
// barometric altimeter in meters and in local NED frame // barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt; Z[9] = BaroAlt;
// EKF correction step // EKF correction step
LinearizeH(ekf.X, ekf.Be, ekf.H); LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(ekf.X, ekf.Be, Y); MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] /= qmag;
ekf.X[7] /= qmag;
ekf.X[8] /= qmag;
ekf.X[9] /= qmag;
invBmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invBmag;
ekf.X[7] *= invBmag;
ekf.X[8] *= invBmag;
ekf.X[9] *= invBmag;
// Update Nav solution structure // Update Nav solution structure
Nav.Pos[0] = ekf.X[0]; Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = ekf.X[1]; Nav.Pos[1] = ekf.X[1];
@ -440,56 +438,74 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
{ {
// Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')] // Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
float dT1 = 1.0f / dT; // multiplication is faster than division on fpu. const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
float dTsq = dT * dT; const float dTsq = dT * dT;
float Dummy[NUMX][NUMX]; float Dummy[NUMX][NUMX];
int8_t i; int8_t i;
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];
float *Dirow = Dummy[i]; float *Dirow = Dummy[i];
int8_t Fistart = FrowMin[i]; const int8_t Fistart = FrowMin[i];
int8_t Fiend = FrowMax[i]; const int8_t Fiend = FrowMax[i];
int8_t j; int8_t j;
for (j = 0; j < NUMX; j++) { for (j = 0; j < NUMX; j++) {
Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ... Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
int8_t k; }
for (k = Fistart; k <= Fiend; k++) { for (k = Fistart; k <= Fiend; k++) {
for (j = 0; j < NUMX; j++) {
Dirow[j] += Firow[k] * P[k][j]; // [] + F * P Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
} }
} }
} }
for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G'] for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
float *Dirow = Dummy[i]; float *Dirow = Dummy[i];
float *Girow = G[i]; float *Girow = G[i];
float *Pirow = P[i]; float *Pirow = P[i];
int8_t Gistart = GrowMin[i]; const int8_t Gistart = GrowMin[i];
int8_t Giend = GrowMax[i]; const int8_t Giend = GrowMax[i];
int8_t j; int8_t j;
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];
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;
int8_t k;
for (k = Fjstart; k <= Fjend; k++) { while (k <= Fjend - 3) {
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ... Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
} Ptmp += Dirow[k + 1] * Fjrow[k + 1];
Ptmp += Dirow[k + 2] * Fjrow[k + 2];
Ptmp += Dirow[k + 3] * Fjrow[k + 3];
k+=4;
}
while (k <= Fjend) {
Ptmp += Dirow[k] * Fjrow[k];
k++;
} }
{ float *Gjrow = G[j];
float *Gjrow = G[j]; const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
int8_t Gjstart = MAX(Gistart, GrowMin[j]); const int8_t Gjend = MIN(Giend, GrowMax[j]);
int8_t Gjend = MIN(Giend, GrowMax[j]); k = Gjstart;
int8_t k; while (k <= Gjend - 2) {
for (k = Gjstart; k <= Gjend; k++) { 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 + 2] * Girow[k + 2] * Gjrow[k + 2];
k+=3;
} }
if (k <= Gjend) {
Ptmp += Q[k] * Girow[k] * Gjrow[k];
if (k <= Gjend - 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)
} }
@ -560,8 +576,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
void RungeKutta(float X[NUMX], float U[NUMU], float dT) void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{ {
float dT2 = const float dT2 = dT / 2.0f;
dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i; uint8_t i;
for (i = 0; i < NUMX; i++) { for (i = 0; i < NUMX; i++) {
@ -608,7 +624,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
// H is output of LinearizeH(), all elements not set should be zero // H is output of LinearizeH(), all elements not set should be zero
// ************************************************ // ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{ {
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3; float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;