jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

(Mostly) remove Hungarian notation

Browse source
This commit is contained in:
PaulStoffregen 2016-03-13 07:45:41 -07:00
commit d41f607b20
8 changed files with 527 additions and 530 deletions
Download patch file Download diff file Expand all files Collapse all files

418
fusion.c
View file

@ -1,7 +1,7 @@
// Copyright (c) 2014, Freescale Semiconductor, Inc.
// All rights reserved.
// vim: set ts=4:
//
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
@ -12,7 +12,7 @@
// * Neither the name of Freescale Semiconductor, Inc. nor the
// names of its contributors may be used to endorse or promote products
// derived from this software without specific prior written permission.
//
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
@ -61,10 +61,6 @@
#define ONEOVER3840 0.0002604166667F // 1 / 3840
#define SENSORFS 100
#define OVERSAMPLE_RATIO 4
static void fqAeq1(Quaternion_t *pqA);
static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]);
@ -86,68 +82,69 @@ static float fatan_15deg(float x);
// function initializes the 9DOF Kalman filter
void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t iOverSampleRatio)
void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV)
{
int8_t i, j; // loop counters
// reset the flag denoting that a first 9DOF orientation lock has been achieved
SV->iFirstOrientationLock = 0;
SV->FirstOrientationLock = 0;
// compute and store useful product terms to save floating point calculations later
SV->fFastdeltat = 1.0F / (float) iSensorFS;
SV->fdeltat = (float) iOverSampleRatio * SV->fFastdeltat;
SV->fdeltatsq = SV->fdeltat * SV->fdeltat;
SV->fcasq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
SV->fcdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
SV->fQwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
SV->Fastdeltat = 1.0F / (float)SENSORFS;
SV->deltat = (float)OVERSAMPLE_RATIO * SV->Fastdeltat;
SV->deltatsq = SV->deltat * SV->deltat;
SV->casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
SV->cdsq = FCD_9DOF_GBY_KALMAN * FCD_9DOF_GBY_KALMAN;
SV->QwbplusQvG = FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN;
// initialize the fixed entries in the measurement matrix C
for (i = 0; i < 6; i++) {
for (j = 0; j < 12; j++) {
SV->fC6x12[i][j]= 0.0F;
SV->C6x12[i][j]= 0.0F;
}
}
SV->fC6x12[0][6] = SV->fC6x12[1][7] = SV->fC6x12[2][8] = 1.0F;
SV->fC6x12[3][9] = SV->fC6x12[4][10] = SV->fC6x12[5][11] = -1.0F;
SV->C6x12[0][6] = SV->C6x12[1][7] = SV->C6x12[2][8] = 1.0F;
SV->C6x12[3][9] = SV->C6x12[4][10] = SV->C6x12[5][11] = -1.0F;
// zero a posteriori orientation, error vector xe+ (thetae+, be+, de+, ae+) and b+ and inertial
f3x3matrixAeqI(SV->fRPl);
fqAeq1(&(SV->fqPl));
f3x3matrixAeqI(SV->RPl);
fqAeq1(&(SV->qPl));
for (i = X; i <= Z; i++) {
SV->fThErrPl[i] = SV->fbErrPl[i] = SV->faErrSePl[i] = SV->fdErrSePl[i] = SV->fbPl[i] = 0.0F;
SV->ThErrPl[i] = SV->bErrPl[i] = SV->aErrSePl[i] = SV->dErrSePl[i] = SV->bPl[i] = 0.0F;
}
// initialize the reference geomagnetic vector (uT, global frame)
SV->fDeltaPl = 0.0F;
SV->DeltaPl = 0.0F;
// initialize NED geomagnetic vector to zero degrees inclination
SV->fmGl[X] = DEFAULTB;
SV->fmGl[Y] = 0.0F;
SV->fmGl[Z] = 0.0F;
SV->mGl[X] = DEFAULTB;
SV->mGl[Y] = 0.0F;
SV->mGl[Z] = 0.0F;
// initialize noise variances for Qv and Qw matrix updates
SV->fQvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->fdeltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
SV->fQvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->fdeltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
SV->QvAA = FQVA_9DOF_GBY_KALMAN + FQWA_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->deltatsq * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
SV->QvMM = FQVM_9DOF_GBY_KALMAN + FQWD_9DOF_GBY_KALMAN + FDEGTORAD * FDEGTORAD * SV->deltatsq * DEFAULTB * DEFAULTB * (FQWB_9DOF_GBY_KALMAN + FQVG_9DOF_GBY_KALMAN);
// initialize the 12x12 noise covariance matrix Qw of the a priori error vector xe-
// Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw updated from P+
// Qw is then recursively updated as P+ = (1 - K * C) * P- = (1 - K * C) * Qw and Qw
// updated from P+
// zero the matrix Qw
for (i = 0; i < 12; i++) {
for (j = 0; j < 12; j++) {
SV->fQw12x12[i][j] = 0.0F;
SV->Qw12x12[i][j] = 0.0F;
}
}
// loop over non-zero values
for (i = 0; i < 3; i++) {
// theta_e * theta_e terms
SV->fQw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
SV->Qw12x12[i][i] = FQWINITTHTH_9DOF_GBY_KALMAN;
// b_e * b_e terms
SV->fQw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 3][i + 3] = FQWINITBB_9DOF_GBY_KALMAN;
// th_e * b_e terms
SV->fQw12x12[i][i + 3] = SV->fQw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
SV->Qw12x12[i][i + 3] = SV->Qw12x12[i + 3][i] = FQWINITTHB_9DOF_GBY_KALMAN;
// a_e * a_e terms
SV->fQw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 6][i + 6] = FQWINITAA_9DOF_GBY_KALMAN;
// d_e * d_e terms
SV->fQw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 9][i + 9] = FQWINITDD_9DOF_GBY_KALMAN;
}
// clear the reset flag
@ -160,8 +157,8 @@ void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t
// 9DOF orientation function implemented using a 12 element Kalman filter
void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
const MagCalibration_t *MagCal, int16_t iOverSampleRatio)
{
const MagCalibration_t *MagCal)
{
// local scalars and arrays
float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse
float fsindelta, fcosdelta; // sin and cos of inclination angle delta
@ -193,7 +190,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// do a reset and return if requested
if (SV->resetflag) {
fInit_9DOF_GBY_KALMAN(SV, SENSORFS, OVERSAMPLE_RATIO);
fInit_9DOF_GBY_KALMAN(SV);
return;
}
@ -201,16 +198,16 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// initial orientation lock to accelerometer and magnetometer eCompass orientation
// *********************************************************************************
// do a once-only orientation lock after the first valid magnetic calibration
if (MagCal->iValidMagCal && !SV->iFirstOrientationLock) {
// do a once-only orientation lock after the first valid magnetic calibration
if (MagCal->ValidMagCal && !SV->FirstOrientationLock) {
// get the 6DOF orientation matrix and initial inclination angle
feCompassNED(SV->fRPl, &(SV->fDeltaPl), Mag->fBcFast, Accel->fGpFast);
feCompassNED(SV->RPl, &(SV->DeltaPl), Mag->BcFast, Accel->GpFast);
// get the orientation quaternion from the orientation matrix
fQuaternionFromRotationMatrix(SV->fRPl, &(SV->fqPl));
fQuaternionFromRotationMatrix(SV->RPl, &(SV->qPl));
// set the orientation lock flag so this initial alignment is only performed once
SV->iFirstOrientationLock = 1;
SV->FirstOrientationLock = 1;
}
// *********************************************************************************
@ -219,31 +216,31 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// compute the angular velocity from the averaged high frequency gyro reading.
// omega[k] = yG[k] - b-[k] = yG[k] - b+[k-1] (deg/s)
SV->fOmega[X] = Gyro->fYp[X] - SV->fbPl[X];
SV->fOmega[Y] = Gyro->fYp[Y] - SV->fbPl[Y];
SV->fOmega[Z] = Gyro->fYp[Z] - SV->fbPl[Z];
SV->Omega[X] = Gyro->Yp[X] - SV->bPl[X];
SV->Omega[Y] = Gyro->Yp[Y] - SV->bPl[Y];
SV->Omega[Z] = Gyro->Yp[Z] - SV->bPl[Z];
// initialize the a priori orientation quaternion to the previous a posteriori estimate
SV->fqMi = SV->fqPl;
SV->qMi = SV->qPl;
// integrate the buffered high frequency (typically 200Hz) gyro readings
for (j = 0; j < iOverSampleRatio; j++) {
for (j = 0; j < OVERSAMPLE_RATIO; j++) {
// compute the incremental fast (typically 200Hz) rotation vector rvec (deg)
for (i = X; i <= Z; i++) {
rvec[i] = (((float)Gyro->iYpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->fbPl[i])
* SV->fFastdeltat;
rvec[i] = (((float)Gyro->YpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->bPl[i])
* SV->Fastdeltat;
}
// compute the incremental quaternion fDeltaq from the rotation vector
fQuaternionFromRotationVectorDeg(&(SV->fDeltaq), rvec, 1.0F);
fQuaternionFromRotationVectorDeg(&(SV->Deltaq), rvec, 1.0F);
// incrementally rotate the a priori orientation quaternion fqMi
// the a posteriori quaternion fqPl is re-normalized later so this update is stable
qAeqAxB(&(SV->fqMi), &(SV->fDeltaq));
qAeqAxB(&(SV->qMi), &(SV->Deltaq));
}
// get the a priori rotation matrix from the a priori quaternion
fRotationMatrixFromQuaternion(SV->fRMi, &(SV->fqMi));
fRotationMatrixFromQuaternion(SV->RMi, &(SV->qMi));
// *********************************************************************************
// calculate a priori gyro, accelerometer and magnetometer estimates
@ -255,22 +252,25 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// compute the a priori gyro estimate of the gravitational vector (g, sensor frame)
// using an absolute rotation of the global frame gravity vector (with magnitude 1g)
// NED gravity is along positive z axis
SV->fgSeGyMi[i] = SV->fRMi[i][Z];
SV->gSeGyMi[i] = SV->RMi[i][Z];
// compute a priori acceleration (a-) (g, sensor frame) from decayed a posteriori estimate (g, sensor frame)
SV->faSeMi[i] = FCA_9DOF_GBY_KALMAN * SV->faSePl[i];
// compute a priori acceleration (a-) (g, sensor frame) from decayed a
// posteriori estimate (g, sensor frame)
SV->aSeMi[i] = FCA_9DOF_GBY_KALMAN * SV->aSePl[i];
// compute the a priori gravity error vector (accelerometer minus gyro estimates) (g, sensor frame)
// compute the a priori gravity error vector (accelerometer minus gyro estimates)
// (g, sensor frame)
// NED and Windows 8 have positive sign for gravity: y = g - a and g = y + a
SV->fgErrSeMi[i] = Accel->fGpFast[i] + SV->faSeMi[i] - SV->fgSeGyMi[i];
SV->gErrSeMi[i] = Accel->GpFast[i] + SV->aSeMi[i] - SV->gSeGyMi[i];
// compute the a priori gyro estimate of the geomagnetic vector (uT, sensor frame)
// using an absolute rotation of the global frame geomagnetic vector (with magnitude B uT)
// NED y component of geomagnetic vector in global frame is zero
SV->fmSeGyMi[i] = SV->fRMi[i][X] * SV->fmGl[X] + SV->fRMi[i][Z] * SV->fmGl[Z];
SV->mSeGyMi[i] = SV->RMi[i][X] * SV->mGl[X] + SV->RMi[i][Z] * SV->mGl[Z];
// compute the a priori geomagnetic error vector (magnetometer minus gyro estimates) (g, sensor frame)
SV->fmErrSeMi[i] = Mag->fBcFast[i] - SV->fmSeGyMi[i];
// compute the a priori geomagnetic error vector (magnetometer minus gyro estimates)
// (g, sensor frame)
SV->mErrSeMi[i] = Mag->BcFast[i] - SV->mSeGyMi[i];
}
// *********************************************************************************
@ -278,30 +278,30 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// *********************************************************************************
// update measurement matrix C with -alpha(g-)x and -alpha(m-)x from gyro (g, uT, sensor frame)
SV->fC6x12[0][1] = FDEGTORAD * SV->fgSeGyMi[Z];
SV->fC6x12[0][2] = -FDEGTORAD * SV->fgSeGyMi[Y];
SV->fC6x12[1][2] = FDEGTORAD * SV->fgSeGyMi[X];
SV->fC6x12[1][0] = -SV->fC6x12[0][1];
SV->fC6x12[2][0] = -SV->fC6x12[0][2];
SV->fC6x12[2][1] = -SV->fC6x12[1][2];
SV->fC6x12[3][1] = FDEGTORAD * SV->fmSeGyMi[Z];
SV->fC6x12[3][2] = -FDEGTORAD * SV->fmSeGyMi[Y];
SV->fC6x12[4][2] = FDEGTORAD * SV->fmSeGyMi[X];
SV->fC6x12[4][0] = -SV->fC6x12[3][1];
SV->fC6x12[5][0] = -SV->fC6x12[3][2];
SV->fC6x12[5][1] = -SV->fC6x12[4][2];
SV->fC6x12[0][4] = -SV->fdeltat * SV->fC6x12[0][1];
SV->fC6x12[0][5] = -SV->fdeltat * SV->fC6x12[0][2];
SV->fC6x12[1][5] = -SV->fdeltat * SV->fC6x12[1][2];
SV->fC6x12[1][3]= -SV->fC6x12[0][4];
SV->fC6x12[2][3]= -SV->fC6x12[0][5];
SV->fC6x12[2][4]= -SV->fC6x12[1][5];
SV->fC6x12[3][4] = -SV->fdeltat * SV->fC6x12[3][1];
SV->fC6x12[3][5] = -SV->fdeltat * SV->fC6x12[3][2];
SV->fC6x12[4][5] = -SV->fdeltat * SV->fC6x12[4][2];
SV->fC6x12[4][3] = -SV->fC6x12[3][4];
SV->fC6x12[5][3] = -SV->fC6x12[3][5];
SV->fC6x12[5][4] = -SV->fC6x12[4][5];
SV->C6x12[0][1] = FDEGTORAD * SV->gSeGyMi[Z];
SV->C6x12[0][2] = -FDEGTORAD * SV->gSeGyMi[Y];
SV->C6x12[1][2] = FDEGTORAD * SV->gSeGyMi[X];
SV->C6x12[1][0] = -SV->C6x12[0][1];
SV->C6x12[2][0] = -SV->C6x12[0][2];
SV->C6x12[2][1] = -SV->C6x12[1][2];
SV->C6x12[3][1] = FDEGTORAD * SV->mSeGyMi[Z];
SV->C6x12[3][2] = -FDEGTORAD * SV->mSeGyMi[Y];
SV->C6x12[4][2] = FDEGTORAD * SV->mSeGyMi[X];
SV->C6x12[4][0] = -SV->C6x12[3][1];
SV->C6x12[5][0] = -SV->C6x12[3][2];
SV->C6x12[5][1] = -SV->C6x12[4][2];
SV->C6x12[0][4] = -SV->deltat * SV->C6x12[0][1];
SV->C6x12[0][5] = -SV->deltat * SV->C6x12[0][2];
SV->C6x12[1][5] = -SV->deltat * SV->C6x12[1][2];
SV->C6x12[1][3]= -SV->C6x12[0][4];
SV->C6x12[2][3]= -SV->C6x12[0][5];
SV->C6x12[2][4]= -SV->C6x12[1][5];
SV->C6x12[3][4] = -SV->deltat * SV->C6x12[3][1];
SV->C6x12[3][5] = -SV->deltat * SV->C6x12[3][2];
SV->C6x12[4][5] = -SV->deltat * SV->C6x12[4][2];
SV->C6x12[4][3] = -SV->C6x12[3][4];
SV->C6x12[5][3] = -SV->C6x12[3][5];
SV->C6x12[5][4] = -SV->C6x12[4][5];
// *********************************************************************************
// calculate the Kalman gain matrix K
@ -322,10 +322,10 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
*pftmpA12x6ij = 0.0F;
// initialize pfC6x12jk for current j, k=0
pfC6x12jk = SV->fC6x12[j];
pfC6x12jk = SV->C6x12[j];
// initialize pfQw12x12ik for current i, k=0
pfQw12x12ik = SV->fQw12x12[i];
pfQw12x12ik = SV->Qw12x12[i];
// sum matrix products over inner loop over k
for (k = 0; k < 12; k++) {
@ -342,27 +342,27 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
pfC6x12jk++;
pfQw12x12ik++;
} // end of loop over k
}
// increment pftmpA12x6ij for next iteration of j
pftmpA12x6ij++;
} // end of loop over j
} // end of loop over i
}
}
// set symmetric P+ (6x6 scratch sub-matrix) to C * P- * C^T + Qv
// = C * (Qw * C^T) + Qv = C * ftmpA12x6 + Qv
// both C and ftmpA12x6 are sparse but not symmetric
for (i = 0; i < 6; i++) { // loop over rows of P+
// initialize pfPPlus12x12ij for current i, j=i
pfPPlus12x12ij = SV->fPPlus12x12[i] + i;
pfPPlus12x12ij = SV->PPlus12x12[i] + i;
for (j = i; j < 6; j++) { // loop over above diagonal columns of P+
// zero P+[i][j]
// zero P+[i][j]
*pfPPlus12x12ij = 0.0F;
// initialize pfC6x12ik for current i, k=0
pfC6x12ik = SV->fC6x12[i];
pfC6x12ik = SV->C6x12[i];
// initialize pftmpA12x6kj for current j, k=0
pftmpA12x6kj = *ftmpA12x6 + j;
@ -379,33 +379,33 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
}
// update pfC6x12ik and pftmpA12x6kj for next iteration of k
pfC6x12ik++;
pfC6x12ik++;
pftmpA12x6kj += 6;
} // end of loop over k
}
// increment pfPPlus12x12ij for next iteration of j
pfPPlus12x12ij++;
} // end of loop over j
} // end of loop over i
}
}
// add in noise covariance terms to the diagonal
SV->fPPlus12x12[0][0] += SV->fQvAA;
SV->fPPlus12x12[1][1] += SV->fQvAA;
SV->fPPlus12x12[2][2] += SV->fQvAA;
SV->fPPlus12x12[3][3] += SV->fQvMM;
SV->fPPlus12x12[4][4] += SV->fQvMM;
SV->fPPlus12x12[5][5] += SV->fQvMM;
SV->PPlus12x12[0][0] += SV->QvAA;
SV->PPlus12x12[1][1] += SV->QvAA;
SV->PPlus12x12[2][2] += SV->QvAA;
SV->PPlus12x12[3][3] += SV->QvMM;
SV->PPlus12x12[4][4] += SV->QvMM;
SV->PPlus12x12[5][5] += SV->QvMM;
// copy above diagonal elements of P+ (6x6 scratch sub-matrix) to below diagonal
for (i = 1; i < 6; i++)
for (j = 0; j < i; j++)
SV->fPPlus12x12[i][j] = SV->fPPlus12x12[j][i];
for (i = 1; i < 6; i++)
for (j = 0; j < i; j++)
SV->PPlus12x12[i][j] = SV->PPlus12x12[j][i];
// calculate inverse of P+ (6x6 scratch sub-matrix) = inv(C * P- * C^T + Qv) = inv(C * Qw * C^T + Qv)
for (i = 0; i < 6; i++) {
pfRows[i] = SV->fPPlus12x12[i];
pfRows[i] = SV->PPlus12x12[i];
}
fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3);
@ -415,7 +415,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// K is not symmetric because C is not symmetric
for (i = 0; i < 12; i++) { // loop over rows of K12x6
// initialize pfK12x6ij for current i, j=0
pfK12x6ij = SV->fK12x6[i];
pfK12x6ij = SV->K12x6[i];
for (j = 0; j < 6; j++) { // loop over columns of K12x6
// zero the matrix element fK12x6[i][j]
@ -425,7 +425,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
pftmpA12x6ik = ftmpA12x6[i];
// initialize pfPPlus12x12kj for current j, k=0
pfPPlus12x12kj = *SV->fPPlus12x12 + j;
pfPPlus12x12kj = *SV->PPlus12x12 + j;
// sum matrix products over inner loop over k
for (k = 0; k < 6; k++) {
@ -437,13 +437,13 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
pftmpA12x6ik++;
pfPPlus12x12kj += 12;
} // end of loop over k
}
// increment pfK12x6ij for the next iteration of j
pfK12x6ij++;
} // end of loop over j
} // end of loop over i
}
}
// *********************************************************************************
// calculate a posteriori error estimate: xe+ = K * ze-
@ -452,40 +452,40 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// first calculate all four error vector components using accelerometer error component only
// for fThErrPl, fbErrPl, faErrSePl but also magnetometer for fdErrSePl
for (i = X; i <= Z; i++) {
SV->fThErrPl[i] = SV->fK12x6[i][0] * SV->fgErrSeMi[X] +
SV->fK12x6[i][1] * SV->fgErrSeMi[Y] +
SV->fK12x6[i][2] * SV->fgErrSeMi[Z];
SV->fbErrPl[i] = SV->fK12x6[i + 3][0] * SV->fgErrSeMi[X] +
SV->fK12x6[i + 3][1] * SV->fgErrSeMi[Y] +
SV->fK12x6[i + 3][2] * SV->fgErrSeMi[Z];
SV->faErrSePl[i] = SV->fK12x6[i + 6][0] * SV->fgErrSeMi[X] +
SV->fK12x6[i + 6][1] * SV->fgErrSeMi[Y] +
SV->fK12x6[i + 6][2] * SV->fgErrSeMi[Z];
SV->fdErrSePl[i] = SV->fK12x6[i + 9][0] * SV->fgErrSeMi[X] +
SV->fK12x6[i + 9][1] * SV->fgErrSeMi[Y] +
SV->fK12x6[i + 9][2] * SV->fgErrSeMi[Z] +
SV->fK12x6[i + 9][3] * SV->fmErrSeMi[X] +
SV->fK12x6[i + 9][4] * SV->fmErrSeMi[Y] +
SV->fK12x6[i + 9][5] * SV->fmErrSeMi[Z];
SV->ThErrPl[i] = SV->K12x6[i][0] * SV->gErrSeMi[X] +
SV->K12x6[i][1] * SV->gErrSeMi[Y] +
SV->K12x6[i][2] * SV->gErrSeMi[Z];
SV->bErrPl[i] = SV->K12x6[i + 3][0] * SV->gErrSeMi[X] +
SV->K12x6[i + 3][1] * SV->gErrSeMi[Y] +
SV->K12x6[i + 3][2] * SV->gErrSeMi[Z];
SV->aErrSePl[i] = SV->K12x6[i + 6][0] * SV->gErrSeMi[X] +
SV->K12x6[i + 6][1] * SV->gErrSeMi[Y] +
SV->K12x6[i + 6][2] * SV->gErrSeMi[Z];
SV->dErrSePl[i] = SV->K12x6[i + 9][0] * SV->gErrSeMi[X] +
SV->K12x6[i + 9][1] * SV->gErrSeMi[Y] +
SV->K12x6[i + 9][2] * SV->gErrSeMi[Z] +
SV->K12x6[i + 9][3] * SV->mErrSeMi[X] +
SV->K12x6[i + 9][4] * SV->mErrSeMi[Y] +
SV->K12x6[i + 9][5] * SV->mErrSeMi[Z];
}
// set the magnetic jamming flag if there is a significant magnetic error power after calibration
ftmp = SV->fdErrSePl[X] * SV->fdErrSePl[X] + SV->fdErrSePl[Y] * SV->fdErrSePl[Y] +
SV->fdErrSePl[Z] * SV->fdErrSePl[Z];
iMagJamming = (MagCal->iValidMagCal) && (ftmp > MagCal->fFourBsq);
ftmp = SV->dErrSePl[X] * SV->dErrSePl[X] + SV->dErrSePl[Y] * SV->dErrSePl[Y] +
SV->dErrSePl[Z] * SV->dErrSePl[Z];
iMagJamming = (MagCal->ValidMagCal) && (ftmp > MagCal->FourBsq);
// add the remaining magnetic error terms if there is calibration and no magnetic jamming
if (MagCal->iValidMagCal && !iMagJamming) {
if (MagCal->ValidMagCal && !iMagJamming) {
for (i = X; i <= Z; i++) {
SV->fThErrPl[i] += SV->fK12x6[i][3] * SV->fmErrSeMi[X] +
SV->fK12x6[i][4] * SV->fmErrSeMi[Y] +
SV->fK12x6[i][5] * SV->fmErrSeMi[Z];
SV->fbErrPl[i] += SV->fK12x6[i + 3][3] * SV->fmErrSeMi[X] +
SV->fK12x6[i + 3][4] * SV->fmErrSeMi[Y] +
SV->fK12x6[i + 3][5] * SV->fmErrSeMi[Z];
SV->faErrSePl[i] += SV->fK12x6[i + 6][3] * SV->fmErrSeMi[X] +
SV->fK12x6[i + 6][4] * SV->fmErrSeMi[Y] +
SV->fK12x6[i + 6][5] * SV->fmErrSeMi[Z];
SV->ThErrPl[i] += SV->K12x6[i][3] * SV->mErrSeMi[X] +
SV->K12x6[i][4] * SV->mErrSeMi[Y] +
SV->K12x6[i][5] * SV->mErrSeMi[Z];
SV->bErrPl[i] += SV->K12x6[i + 3][3] * SV->mErrSeMi[X] +
SV->K12x6[i + 3][4] * SV->mErrSeMi[Y] +
SV->K12x6[i + 3][5] * SV->mErrSeMi[Z];
SV->aErrSePl[i] += SV->K12x6[i + 6][3] * SV->mErrSeMi[X] +
SV->K12x6[i + 6][4] * SV->mErrSeMi[Y] +
SV->K12x6[i + 6][5] * SV->mErrSeMi[Z];
}
}
@ -494,61 +494,61 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// *********************************************************************************
// get the a posteriori delta quaternion
fQuaternionFromRotationVectorDeg(&(SV->fDeltaq), SV->fThErrPl, -1.0F);
fQuaternionFromRotationVectorDeg(&(SV->Deltaq), SV->ThErrPl, -1.0F);
// compute the a posteriori orientation quaternion fqPl = fqMi * Deltaq(-thetae+)
// the resulting quaternion may have negative scalar component q0
qAeqBxC(&(SV->fqPl), &(SV->fqMi), &(SV->fDeltaq));
qAeqBxC(&(SV->qPl), &(SV->qMi), &(SV->Deltaq));
// normalize the a posteriori orientation quaternion to stop error propagation
// normalize the a posteriori orientation quaternion to stop error propagation
// the renormalization function ensures that the scalar component q0 is non-negative
fqAeqNormqA(&(SV->fqPl));
fqAeqNormqA(&(SV->qPl));
// compute the a posteriori rotation matrix from the a posteriori quaternion
fRotationMatrixFromQuaternion(SV->fRPl, &(SV->fqPl));
fRotationMatrixFromQuaternion(SV->RPl, &(SV->qPl));
// compute the rotation vector from the a posteriori quaternion
fRotationVectorDegFromQuaternion(&(SV->fqPl), SV->fRVecPl);
fRotationVectorDegFromQuaternion(&(SV->qPl), SV->RVecPl);
// update the a posteriori gyro offset vector b+ and
// assign the entire linear acceleration error vector to update the linear acceleration
for (i = X; i <= Z; i++) {
// b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
SV->fbPl[i] -= SV->fbErrPl[i];
SV->bPl[i] -= SV->bErrPl[i];
// a+ = a- - ae+ (g, sensor frame)
SV->faSePl[i] = SV->faSeMi[i] - SV->faErrSePl[i];
SV->aSePl[i] = SV->aSeMi[i] - SV->aErrSePl[i];
}
// compute the linear acceleration in the global frame from the accelerometer measurement (sensor frame).
// de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose)
// de-rotate the accelerometer measurement from the sensor to global frame using the inverse (transpose)
// of the a posteriori rotation matrix
SV->faGlPl[X] = SV->fRPl[X][X] * Accel->fGpFast[X] + SV->fRPl[Y][X] * Accel->fGpFast[Y] +
SV->fRPl[Z][X] * Accel->fGpFast[Z];
SV->faGlPl[Y] = SV->fRPl[X][Y] * Accel->fGpFast[X] + SV->fRPl[Y][Y] * Accel->fGpFast[Y] +
SV->fRPl[Z][Y] * Accel->fGpFast[Z];
SV->faGlPl[Z] = SV->fRPl[X][Z] * Accel->fGpFast[X] + SV->fRPl[Y][Z] * Accel->fGpFast[Y] +
SV->fRPl[Z][Z] * Accel->fGpFast[Z];
// remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative
SV->aGlPl[X] = SV->RPl[X][X] * Accel->GpFast[X] + SV->RPl[Y][X] * Accel->GpFast[Y] +
SV->RPl[Z][X] * Accel->GpFast[Z];
SV->aGlPl[Y] = SV->RPl[X][Y] * Accel->GpFast[X] + SV->RPl[Y][Y] * Accel->GpFast[Y] +
SV->RPl[Z][Y] * Accel->GpFast[Z];
SV->aGlPl[Z] = SV->RPl[X][Z] * Accel->GpFast[X] + SV->RPl[Y][Z] * Accel->GpFast[Y] +
SV->RPl[Z][Z] * Accel->GpFast[Z];
// remove gravity and correct the sign if the coordinate system is gravity positive / acceleration negative
// gravity positive NED
SV->faGlPl[X] = -SV->faGlPl[X];
SV->faGlPl[Y] = -SV->faGlPl[Y];
SV->faGlPl[Z] = -(SV->faGlPl[Z] - 1.0F);
SV->aGlPl[X] = -SV->aGlPl[X];
SV->aGlPl[Y] = -SV->aGlPl[Y];
SV->aGlPl[Z] = -(SV->aGlPl[Z] - 1.0F);
// update the reference geomagnetic vector using magnetic disturbance error if valid calibration and no jamming
if (MagCal->iValidMagCal && !iMagJamming) {
if (MagCal->ValidMagCal && !iMagJamming) {
// de-rotate the NED magnetic disturbance error de+ from the sensor to the global reference frame
// using the inverse (transpose) of the a posteriori rotation matrix
SV->fdErrGlPl[X] = SV->fRPl[X][X] * SV->fdErrSePl[X] +
SV->fRPl[Y][X] * SV->fdErrSePl[Y] +
SV->fRPl[Z][X] * SV->fdErrSePl[Z];
SV->fdErrGlPl[Z] = SV->fRPl[X][Z] * SV->fdErrSePl[X] +
SV->fRPl[Y][Z] * SV->fdErrSePl[Y] +
SV->fRPl[Z][Z] * SV->fdErrSePl[Z];
SV->dErrGlPl[X] = SV->RPl[X][X] * SV->dErrSePl[X] +
SV->RPl[Y][X] * SV->dErrSePl[Y] +
SV->RPl[Z][X] * SV->dErrSePl[Z];
SV->dErrGlPl[Z] = SV->RPl[X][Z] * SV->dErrSePl[X] +
SV->RPl[Y][Z] * SV->dErrSePl[Y] +
SV->RPl[Z][Z] * SV->dErrSePl[Z];
// compute components of the new geomagnetic vector
// the north pointing component fadj must always be non-negative
fopp = SV->fmGl[Z] - SV->fdErrGlPl[Z];
fadj = SV->fmGl[X] - SV->fdErrGlPl[X];
fopp = SV->mGl[Z] - SV->dErrGlPl[Z];
fadj = SV->mGl[X] - SV->dErrGlPl[X];
if (fadj < 0.0F) {
fadj = 0.0F;
}
@ -571,19 +571,19 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
}
// compute the new geomagnetic vector (always north pointing)
SV->fDeltaPl = fasin_deg(fsindelta);
SV->fmGl[X] = MagCal->fB * fcosdelta;
SV->fmGl[Z] = MagCal->fB * fsindelta;
SV->DeltaPl = fasin_deg(fsindelta);
SV->mGl[X] = MagCal->B * fcosdelta;
SV->mGl[Z] = MagCal->B * fsindelta;
} // end hyp == 0.0F
} // end iValidMagCal
} // end ValidMagCal
// *********************************************************************************
// compute the a posteriori Euler angles from the orientation matrix
// *********************************************************************************
// calculate the NED Euler angles
fNEDAnglesDegFromRotationMatrix(SV->fRPl, &(SV->fPhiPl), &(SV->fThePl), &(SV->fPsiPl),
&(SV->fRhoPl), &(SV->fChiPl));
fNEDAnglesDegFromRotationMatrix(SV->RPl, &(SV->PhiPl), &(SV->ThePl), &(SV->PsiPl),
&(SV->RhoPl), &(SV->ChiPl));
// ***********************************************************************************
// calculate (symmetric) a posteriori error covariance matrix P+
@ -597,17 +597,17 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// the resulting matrix is sparse but not symmetric
for (i = 0; i < 6; i++) {
// initialize pfPPlus12x12ij for current i, j=0
pfPPlus12x12ij = SV->fPPlus12x12[i];
pfPPlus12x12ij = SV->PPlus12x12[i];
for (j = 0; j < 12; j++) {
// zero P+[i][j]
// zero P+[i][j]
*pfPPlus12x12ij = 0.0F;
// initialize pfC6x12ik for current i, k=0
pfC6x12ik = SV->fC6x12[i];
pfC6x12ik = SV->C6x12[i];
// initialize pfQw12x12kj for current j, k=0
pfQw12x12kj = &SV->fQw12x12[0][j];
pfQw12x12kj = &SV->Qw12x12[0][j];
// sum matrix products over inner loop over k
for (k = 0; k < 12; k++) {
@ -624,13 +624,13 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
pfC6x12ik++;
pfQw12x12kj += 12;
} // end of loop over k
}
// increment pfPPlus12x12ij for next iteration of j
pfPPlus12x12ij++;
} // end of loop over j
} // end of loop over i
}
}
// compute P+ = (I12 - K * C) * Qw = Qw - K * (C * Qw) = Qw - K * P+ (6x12 sub-matrix)
// storing result P+ in Qw and over-writing Qw which is OK since Qw is later computed from P+
@ -638,14 +638,14 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// only on and above diagonal terms of P+ are computed since P+ is symmetric
for (i = 0; i < 12; i++) {
// initialize pfQw12x12ij for current i, j=i
pfQw12x12ij = SV->fQw12x12[i] + i;
pfQw12x12ij = SV->Qw12x12[i] + i;
for (j = i; j < 12; j++) {
// initialize pfK12x6ik for current i, k=0
pfK12x6ik = SV->fK12x6[i];
pfK12x6ik = SV->K12x6[i];
// initialize pfPPlus12x12kj for current j, k=0
pfPPlus12x12kj = *SV->fPPlus12x12 + j;
pfPPlus12x12kj = *SV->PPlus12x12 + j;
// compute on and above diagonal matrix entry
for (k = 0; k < 6; k++) {
@ -653,34 +653,30 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
if (*pfPPlus12x12kj != 0.0F) {
*pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj;
}
// increment pfK12x6ik and pfPPlus12x12kj for next iteration of k
pfK12x6ik++;
pfPPlus12x12kj += 12;
} // end of loop over k
}
// increment pfQw12x12ij for next iteration of j
pfQw12x12ij++;
} // end of loop over j
} // end of loop over i
}
}
// Qw now holds the on and above diagonal elements of P+
// so perform a simple copy to the all elements of P+
// after execution of this code P+ is valid but Qw remains invalid
for (i = 0; i < 12; i++) {
// initialize pfPPlus12x12ij and pfQw12x12ij for current i, j=i
pfPPlus12x12ij = SV->fPPlus12x12[i] + i;
pfQw12x12ij = SV->fQw12x12[i] + i;
pfPPlus12x12ij = SV->PPlus12x12[i] + i;
pfQw12x12ij = SV->Qw12x12[i] + i;
// copy the on-diagonal elements and increment pointers to enter loop at j=i+1
*(pfPPlus12x12ij++) = *(pfQw12x12ij++);
// loop over above diagonal columns j copying to below-diagonal elements
for (j = i + 1; j < 12; j++)
{
*(pfPPlus12x12ij++)= SV->fPPlus12x12[j][i] = *(pfQw12x12ij++);
for (j = i + 1; j < 12; j++) {
*(pfPPlus12x12ij++)= SV->PPlus12x12[j][i] = *(pfQw12x12ij++);
}
}
@ -693,26 +689,26 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
// zero the matrix Qw
for (i = 0; i < 12; i++) {
for (j = 0; j < 12; j++) {
SV->fQw12x12[i][j] = 0.0F;
SV->Qw12x12[i][j] = 0.0F;
}
}
// update the covariance matrix components
for (i = 0; i < 3; i++) {
// Qw[th-th-] = Qw[0-2][0-2] = E[th-(th-)^T] = Q[th+th+] + deltat^2 * (Q[b+b+] + (Qwb + QvG) * I)
SV->fQw12x12[i][i] = SV->fPPlus12x12[i][i] + SV->fdeltatsq * (SV->fPPlus12x12[i + 3][i + 3] + SV->fQwbplusQvG);
SV->Qw12x12[i][i] = SV->PPlus12x12[i][i] + SV->deltatsq * (SV->PPlus12x12[i + 3][i + 3] + SV->QwbplusQvG);
// Qw[b-b-] = Qw[3-5][3-5] = E[b-(b-)^T] = Q[b+b+] + Qwb * I
SV->fQw12x12[i + 3][i + 3] = SV->fPPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 3][i + 3] = SV->PPlus12x12[i + 3][i + 3] + FQWB_9DOF_GBY_KALMAN;
// Qw[th-b-] = Qw[0-2][3-5] = E[th-(b-)^T] = -deltat * (Q[b+b+] + Qwb * I) = -deltat * Qw[b-b-]
SV->fQw12x12[i][i + 3] = SV->fQw12x12[i + 3][i] = -SV->fdeltat * SV->fQw12x12[i + 3][i + 3];
SV->Qw12x12[i][i + 3] = SV->Qw12x12[i + 3][i] = -SV->deltat * SV->Qw12x12[i + 3][i + 3];
// Qw[a-a-] = Qw[6-8][6-8] = E[a-(a-)^T] = ca^2 * Q[a+a+] + Qwa * I
SV->fQw12x12[i + 6][i + 6] = SV->fcasq * SV->fPPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 6][i + 6] = SV->casq * SV->PPlus12x12[i + 6][i + 6] + FQWA_9DOF_GBY_KALMAN;
// Qw[d-d-] = Qw[9-11][9-11] = E[d-(d-)^T] = cd^2 * Q[d+d+] + Qwd * I
SV->fQw12x12[i + 9][i + 9] = SV->fcdsq * SV->fPPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
SV->Qw12x12[i + 9][i + 9] = SV->cdsq * SV->PPlus12x12[i + 9][i + 9] + FQWD_9DOF_GBY_KALMAN;
}
}
@ -731,7 +727,7 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
void f3DOFTiltNED(float fR[][3], float fGp[])
{
// the NED self-consistency twist occurs at 90 deg pitch
// local variables
int16_t i; // counter
float fmodGxyz; // modulus of the x, y, z accelerometer readings
@ -788,7 +784,7 @@ void f3DOFTiltNED(float fR[][3], float fGp[])
// Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
{
{
// local variables
float fmodBxy; // modulus of the x, y magnetometer readings
@ -804,7 +800,7 @@ void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
// define the fixed entries in the z row and column
fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
fR[Z][Z] = 1.0F;
// define the remaining entries
fR[X][X] = fR[Y][Y] = fBc[X] / fmodBxy;
fR[Y][X] = fBc[Y] / fmodBxy;
@ -912,7 +908,7 @@ static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg,
// for NED, the compass heading Rho equals the yaw angle Psi
*pfRhoDeg = *pfPsiDeg;
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
// calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
*pfChiDeg = facos_deg(R[Z][Z]);
return;
@ -1099,7 +1095,7 @@ void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
if ((R[Y][Z] - R[Z][Y]) < 0.0F) rvecdeg[X] = -rvecdeg[X];
if ((R[Z][X] - R[X][Z]) < 0.0F) rvecdeg[Y] = -rvecdeg[Y];
if ((R[X][Y] - R[Y][X]) < 0.0F) rvecdeg[Z] = -rvecdeg[Z];
}
}
@ -1117,12 +1113,12 @@ static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[])
fetarad = 0.0F;
fetadeg = 0.0F;
} else {
// general case returning 0 < eta < 360 deg
fetarad = 2.0F * acosf(pq->q0);
// general case returning 0 < eta < 360 deg
fetarad = 2.0F * acosf(pq->q0);
fetadeg = fetarad * FRADTODEG;
}
// map the rotation angle onto the range -180 deg <= eta < 180 deg
// map the rotation angle onto the range -180 deg <= eta < 180 deg
if (fetadeg >= 180.0F) {
fetadeg -= 360.0F;
fetarad = fetadeg * FDEGTORAD;
@ -1133,7 +1129,7 @@ static void fRotationVectorDegFromQuaternion(Quaternion_t *pq, float rvecdeg[])
// calculate the rotation vector (deg)
if (sinhalfeta == 0.0F) {
// the rotation angle eta is zero and the axis is irrelevant
// the rotation angle eta is zero and the axis is irrelevant
rvecdeg[X] = rvecdeg[Y] = rvecdeg[Z] = 0.0F;
} else {
// general case with non-zero rotation angle
@ -1339,7 +1335,7 @@ static float fatan_deg(float x)
// reset all flags
ixisnegative = ixexceeds1 = ixmapped = 0;
// test for negative argument to allow use of tan(-x)=-tan(x)
if (x < 0.0F) {
x = -x;
@ -1369,7 +1365,7 @@ static float fatan_deg(float x)
if (ixmapped) fangledeg += 30.0F;
if (ixexceeds1) fangledeg = 90.0F - fangledeg;
if (ixisnegative) fangledeg = -fangledeg;
return (fangledeg);
}
@ -1386,7 +1382,7 @@ static float fatan2_deg(float y, float x)
// otherwise y= 0.0 and return 0 deg (invalid arguments)
return 0.0F;
}
// from here onwards, x is guaranteed to be non-zero
// compute atan2 for quadrant 1 (0 to 90 deg) and quadrant 4 (-90 to 0 deg)
if (x > 0.0F) return (fatan_deg(y / x));