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));

12
imuread.c
View file

@ -23,18 +23,18 @@ static void glut_display_callback(void)
static void glut_keystroke_callback(unsigned char ch, int x, int y)
{
if (magcal.fFitErrorpc > 9.0) {
if (magcal.FitError > 9.0) {
printf("Poor Calibration: ");
printf("soft iron fit error = %.1f%%\n", magcal.fFitErrorpc);
printf("soft iron fit error = %.1f%%\n", magcal.FitError);
return;
}
printf("Magnetic Calibration: (%.1f%% fit error)\n", magcal.fFitErrorpc);
printf("Magnetic Calibration: (%.1f%% fit error)\n", magcal.FitError);
printf(" %7.2f %6.3f %6.3f %6.3f\n",
magcal.fV[0], magcal.finvW[0][0], magcal.finvW[0][1], magcal.finvW[0][2]);
magcal.V[0], magcal.invW[0][0], magcal.invW[0][1], magcal.invW[0][2]);
printf(" %7.2f %6.3f %6.3f %6.3f\n",
magcal.fV[1], magcal.finvW[1][0], magcal.finvW[1][1], magcal.finvW[1][2]);
magcal.V[1], magcal.invW[1][0], magcal.invW[1][1], magcal.invW[1][2]);
printf(" %7.2f %6.3f %6.3f %6.3f\n",
magcal.fV[2], magcal.finvW[2][0], magcal.finvW[2][1], magcal.finvW[2][2]);
magcal.V[2], magcal.invW[2][0], magcal.invW[2][1], magcal.invW[2][2]);
send_calibration();
}

136
imuread.h
View file

@ -76,26 +76,26 @@ void MagCal_Run(void);
// magnetic calibration & buffer structure
typedef struct {
float fV[3]; // current hard iron offset x, y, z, (uT)
float finvW[3][3]; // current inverse soft iron matrix
float fB; // current geomagnetic field magnitude (uT)
float fFourBsq; // current 4*B*B (uT^2)
float fFitErrorpc; // current fit error %
float fFitErrorAge; // current fit error % (grows automatically with age)
float ftrV[3]; // trial value of hard iron offset z, y, z (uT)
float ftrinvW[3][3]; // trial inverse soft iron matrix size
float ftrB; // trial value of geomagnetic field magnitude in uT
float ftrFitErrorpc; // trial value of fit error %
float fA[3][3]; // ellipsoid matrix A
float finvA[3][3]; // inverse of ellipsoid matrix A
float fmatA[10][10]; // scratch 10x10 matrix used by calibration algorithms
float fmatB[10][10]; // scratch 10x10 matrix used by calibration algorithms
float fvecA[10]; // scratch 10x1 vector used by calibration algorithms
float fvecB[4]; // scratch 4x1 vector used by calibration algorithms
int8_t iValidMagCal; // integer value 0, 4, 7, 10 denoting both valid calibration and solver used
int16_t iBpFast[3][MAGBUFFSIZE]; // uncalibrated magnetometer readings
float V[3]; // current hard iron offset x, y, z, (uT)
float invW[3][3]; // current inverse soft iron matrix
float B; // current geomagnetic field magnitude (uT)
float FourBsq; // current 4*B*B (uT^2)
float FitError; // current fit error %
float FitErrorAge; // current fit error % (grows automatically with age)
float trV[3]; // trial value of hard iron offset z, y, z (uT)
float trinvW[3][3]; // trial inverse soft iron matrix size
float trB; // trial value of geomagnetic field magnitude in uT
float trFitErrorpc; // trial value of fit error %
float A[3][3]; // ellipsoid matrix A
float invA[3][3]; // inverse of ellipsoid matrix A
float matA[10][10]; // scratch 10x10 matrix used by calibration algorithms
float matB[10][10]; // scratch 10x10 matrix used by calibration algorithms
float vecA[10]; // scratch 10x1 vector used by calibration algorithms
float vecB[4]; // scratch 4x1 vector used by calibration algorithms
int8_t ValidMagCal; // integer value 0, 4, 7, 10 denoting both valid calibration and solver used
int16_t BpFast[3][MAGBUFFSIZE]; // uncalibrated magnetometer readings
int8_t valid[MAGBUFFSIZE]; // 1=has data, 0=empty slot
int16_t iMagBufferCount; // number of magnetometer readings
int16_t MagBufferCount; // number of magnetometer readings
} MagCalibration_t;
extern MagCalibration_t magcal;
@ -113,15 +113,17 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivo
void fmatrixAeqRenormRotA(float A[][3]);
#define SENSORFS 100
#define OVERSAMPLE_RATIO 4
// accelerometer sensor structure definition
#define G_PER_COUNT 0.0001220703125F // = 1/8192
typedef struct
{
//int32_t iSumGpFast[3]; // sum of fast measurements
float fGpFast[3]; // fast (typically 200Hz) readings (g)
float fGp[3]; // slow (typically 25Hz) averaged readings (g)
float GpFast[3]; // fast (typically 200Hz) readings (g)
float Gp[3]; // slow (typically 25Hz) averaged readings (g)
//float fgPerCount; // initialized to FGPERCOUNT
//int16_t iGpFast[3]; // fast (typically 200Hz) readings
//int16_t iGp[3]; // slow (typically 25Hz) averaged readings (counts)
@ -134,8 +136,8 @@ typedef struct
//int32_t iSumBpFast[3]; // sum of fast measurements
//float fBpFast[3]; // fast (typically 200Hz) raw readings (uT)
//float fBp[3]; // slow (typically 25Hz) averaged raw readings (uT)
float fBcFast[3]; // fast (typically 200Hz) calibrated readings (uT)
float fBc[3]; // slow (typically 25Hz) averaged calibrated readings (uT)
float BcFast[3]; // fast (typically 200Hz) calibrated readings (uT)
float Bc[3]; // slow (typically 25Hz) averaged calibrated readings (uT)
//float fuTPerCount; // initialized to FUTPERCOUNT
//float fCountsPeruT; // initialized to FCOUNTSPERUT
//int16_t iBpFast[3]; // fast (typically 200Hz) raw readings (counts)
@ -148,9 +150,9 @@ typedef struct
typedef struct
{
//int32_t iSumYpFast[3]; // sum of fast measurements
float fYp[3]; // raw gyro sensor output (deg/s)
float Yp[3]; // raw gyro sensor output (deg/s)
//float fDegPerSecPerCount; // initialized to FDEGPERSECPERCOUNT
int16_t iYpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings
int16_t YpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings
//int16_t iYp[3]; // averaged gyro sensor output (counts)
} GyroSensor_t;
@ -161,62 +163,62 @@ typedef struct
{
// start: elements common to all motion state vectors
// Euler angles
float fPhiPl; // roll (deg)
float fThePl; // pitch (deg)
float fPsiPl; // yaw (deg)
float fRhoPl; // compass (deg)
float fChiPl; // tilt from vertical (deg)
float PhiPl; // roll (deg)
float ThePl; // pitch (deg)
float PsiPl; // yaw (deg)
float RhoPl; // compass (deg)
float ChiPl; // tilt from vertical (deg)
// orientation matrix, quaternion and rotation vector
float fRPl[3][3]; // a posteriori orientation matrix
Quaternion_t fqPl; // a posteriori orientation quaternion
float fRVecPl[3]; // rotation vector
float RPl[3][3]; // a posteriori orientation matrix
Quaternion_t qPl; // a posteriori orientation quaternion
float RVecPl[3]; // rotation vector
// angular velocity
float fOmega[3]; // angular velocity (deg/s)
float Omega[3]; // angular velocity (deg/s)
// systick timer for benchmarking
int32_t systick; // systick timer;
// end: elements common to all motion state vectors
// elements transmitted over bluetooth in kalman packet
float fbPl[3]; // gyro offset (deg/s)
float fThErrPl[3]; // orientation error (deg)
float fbErrPl[3]; // gyro offset error (deg/s)
float bPl[3]; // gyro offset (deg/s)
float ThErrPl[3]; // orientation error (deg)
float bErrPl[3]; // gyro offset error (deg/s)
// end elements transmitted in kalman packet
float fdErrGlPl[3]; // magnetic disturbance error (uT, global frame)
float fdErrSePl[3]; // magnetic disturbance error (uT, sensor frame)
float faErrSePl[3]; // linear acceleration error (g, sensor frame)
float faSeMi[3]; // linear acceleration (g, sensor frame)
float fDeltaPl; // inclination angle (deg)
float faSePl[3]; // linear acceleration (g, sensor frame)
float faGlPl[3]; // linear acceleration (g, global frame)
float fgErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
float fmErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
float fgSeGyMi[3]; // gravity vector (g, sensor frame) measurement from gyro
float fmSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro
float fmGl[3]; // geomagnetic vector (uT, global frame)
float fQvAA; // accelerometer terms of Qv
float fQvMM; // magnetometer terms of Qv
float fPPlus12x12[12][12]; // covariance matrix P+
float fK12x6[12][6]; // kalman filter gain matrix K
float fQw12x12[12][12]; // covariance matrix Qw
float fC6x12[6][12]; // measurement matrix C
float fRMi[3][3]; // a priori orientation matrix
Quaternion_t fDeltaq; // delta quaternion
Quaternion_t fqMi; // a priori orientation quaternion
float fcasq; // FCA * FCA;
float fcdsq; // FCD * FCD;
float fFastdeltat; // sensor sampling interval (s) = 1 / SENSORFS
float fdeltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
float fdeltatsq; // fdeltat * fdeltat
float fQwbplusQvG; // FQWB + FQVG
int16_t iFirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
float dErrGlPl[3]; // magnetic disturbance error (uT, global frame)
float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame)
float aErrSePl[3]; // linear acceleration error (g, sensor frame)
float aSeMi[3]; // linear acceleration (g, sensor frame)
float DeltaPl; // inclination angle (deg)
float aSePl[3]; // linear acceleration (g, sensor frame)
float aGlPl[3]; // linear acceleration (g, global frame)
float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel) and gravity vector (gyro)
float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector (magnetometer) and geomagnetic vector (gyro)
float gSeGyMi[3]; // gravity vector (g, sensor frame) measurement from gyro
float mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro
float mGl[3]; // geomagnetic vector (uT, global frame)
float QvAA; // accelerometer terms of Qv
float QvMM; // magnetometer terms of Qv
float PPlus12x12[12][12]; // covariance matrix P+
float K12x6[12][6]; // kalman filter gain matrix K
float Qw12x12[12][12]; // covariance matrix Qw
float C6x12[6][12]; // measurement matrix C
float RMi[3][3]; // a priori orientation matrix
Quaternion_t Deltaq; // delta quaternion
Quaternion_t qMi; // a priori orientation quaternion
float casq; // FCA * FCA;
float cdsq; // FCD * FCD;
float Fastdeltat; // sensor sampling interval (s) = 1 / SENSORFS
float deltat; // kalman filter sampling interval (s) = OVERSAMPLE_RATIO / SENSORFS
float deltatsq; // fdeltat * fdeltat
float QwbplusQvG; // FQWB + FQVG
int16_t FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
int8_t resetflag; // flag to request re-initialization on next pass
} SV_9DOF_GBY_KALMAN_t;
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);
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);

348
magcal.c
View file

@ -44,8 +44,8 @@
#define MINMEASUREMENTS4CAL 40 // minimum number of measurements for 4 element calibration
#define MINMEASUREMENTS7CAL 100 // minimum number of measurements for 7 element calibration
#define MINMEASUREMENTS10CAL 150 // minimum number of measurements for 10 element calibration
#define MINBFITUT 22.0F // minimum geomagnetic field B (uT) for valid calibration
#define MAXBFITUT 67.0F // maximum geomagnetic field B (uT) for valid calibration
#define MINBFITUT 15.0F // minimum geomagnetic field B (uT) for valid calibration
#define MAXBFITUT 80.0F // maximum geomagnetic field B (uT) for valid calibration
#define FITERRORAGINGSECS 7200.0F // 2 hours: time for fit error to increase (age) by e=2.718
static void fUpdateCalibration4INV(MagCalibration_t *MagCal);
@ -73,9 +73,9 @@ void MagCal_Run(void)
if (count < MINMEASUREMENTS4CAL) return;
if (magcal.iValidMagCal) {
if (magcal.ValidMagCal) {
// age the existing fit error to avoid one good calibration locking out future updates
magcal.fFitErrorAge *= 1.01f;
magcal.FitErrorAge *= 1.01f;
}
// is enough data collected
@ -91,25 +91,25 @@ void MagCal_Run(void)
}
// the trial geomagnetic field must be in range (earth is 22uT to 67uT)
if ((magcal.ftrB >= MINBFITUT) && (magcal.ftrB <= MAXBFITUT)) {
if ((magcal.trB >= MINBFITUT) && (magcal.trB <= MAXBFITUT)) {
// always accept the calibration if
// 1: no previous calibration exists
// 2: the calibration fit is reduced or
// 3: an improved solver was used giving a good trial calibration (4% or under)
if ((magcal.iValidMagCal == 0) ||
(magcal.ftrFitErrorpc <= magcal.fFitErrorAge) ||
((isolver > magcal.iValidMagCal) && (magcal.ftrFitErrorpc <= 4.0F))) {
if ((magcal.ValidMagCal == 0) ||
(magcal.trFitErrorpc <= magcal.FitErrorAge) ||
((isolver > magcal.ValidMagCal) && (magcal.trFitErrorpc <= 4.0F))) {
// accept the new calibration solution
//printf("new magnetic cal, B=%.2f uT\n", magcal.ftrB);
magcal.iValidMagCal = isolver;
magcal.fFitErrorpc = magcal.ftrFitErrorpc;
magcal.fFitErrorAge = magcal.ftrFitErrorpc;
magcal.fB = magcal.ftrB;
magcal.fFourBsq = 4.0F * magcal.ftrB * magcal.ftrB;
//printf("new magnetic cal, B=%.2f uT\n", magcal.trB);
magcal.ValidMagCal = isolver;
magcal.FitError = magcal.trFitErrorpc;
magcal.FitErrorAge = magcal.trFitErrorpc;
magcal.B = magcal.trB;
magcal.FourBsq = 4.0F * magcal.trB * magcal.trB;
for (i = X; i <= Z; i++) {
magcal.fV[i] = magcal.ftrV[i];
magcal.V[i] = magcal.trV[i];
for (j = X; j <= Z; j++) {
magcal.finvW[i][j] = magcal.ftrinvW[i][j];
magcal.invW[i][j] = magcal.trinvW[i][j];
}
}
}
@ -128,7 +128,7 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
int16_t iOffset[3]; // offset to remove large DC hard iron bias in matrix
int16_t iCount; // number of measurements counted
int i, j, k; // loop counters
// working arrays for 4x4 matrix inversion
float *pfRows[4];
int8_t iColInd[4];
@ -140,15 +140,15 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
// the trial inverse soft iron matrix invW always equals
// the identity matrix for 4 element calibration
f3x3matrixAeqI(MagCal->ftrinvW);
f3x3matrixAeqI(MagCal->trinvW);
// zero fSumBp4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above
// diagonal elements of fmatA=X^T*X (4x4)
// zero fSumBp4=Y^T.Y, vecB=X^T.Y (4x1) and on and above
// diagonal elements of matA=X^T*X (4x4)
fSumBp4 = 0.0F;
for (i = 0; i < 4; i++) {
MagCal->fvecB[i] = 0.0F;
MagCal->vecB[i] = 0.0F;
for (j = i; j < 4; j++) {
MagCal->fmatA[i][j] = 0.0F;
MagCal->matA[i][j] = 0.0F;
}
}
@ -162,41 +162,41 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
// use first valid magnetic buffer entry as estimate (in counts) for offset
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
iOffset[k] = MagCal->BpFast[k][j];
}
}
// store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
// store scaled and offset fBp[XYZ] in vecA[0-2] and fBp[XYZ]^2 in vecA[3-5]
for (k = X; k <= Z; k++) {
MagCal->fvecA[k] = (float)((int32_t)MagCal->iBpFast[k][j]
MagCal->vecA[k] = (float)((int32_t)MagCal->BpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
MagCal->fvecA[k + 3] = MagCal->fvecA[k] * MagCal->fvecA[k];
MagCal->vecA[k + 3] = MagCal->vecA[k] * MagCal->vecA[k];
}
// calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
fBp2 = MagCal->fvecA[3] + MagCal->fvecA[4] + MagCal->fvecA[5];
// calculate fBp2 = Bp[X]^2 + Bp[Y]^2 + Bp[Z]^2 (scaled uT^2)
fBp2 = MagCal->vecA[3] + MagCal->vecA[4] + MagCal->vecA[5];
// accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
fSumBp4 += fBp2 * fBp2;
// now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
// now we have fBp2, accumulate vecB[0-2] = X^T.Y =sum(Bp2.Bp[XYZ])
for (k = X; k <= Z; k++) {
MagCal->fvecB[k] += MagCal->fvecA[k] * fBp2;
MagCal->vecB[k] += MagCal->vecA[k] * fBp2;
}
//accumulate fvecB[3] = X^T.Y =sum(fBp2)
MagCal->fvecB[3] += fBp2;
//accumulate vecB[3] = X^T.Y =sum(fBp2)
MagCal->vecB[3] += fBp2;
// accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
MagCal->fmatA[0][0] += MagCal->fvecA[X + 3];
MagCal->fmatA[0][1] += MagCal->fvecA[X] * MagCal->fvecA[Y];
MagCal->fmatA[0][2] += MagCal->fvecA[X] * MagCal->fvecA[Z];
MagCal->fmatA[0][3] += MagCal->fvecA[X];
MagCal->fmatA[1][1] += MagCal->fvecA[Y + 3];
MagCal->fmatA[1][2] += MagCal->fvecA[Y] * MagCal->fvecA[Z];
MagCal->fmatA[1][3] += MagCal->fvecA[Y];
MagCal->fmatA[2][2] += MagCal->fvecA[Z + 3];
MagCal->fmatA[2][3] += MagCal->fvecA[Z];
// accumulate on and above-diagonal terms of matA = X^T.X ignoring matA[3][3]
MagCal->matA[0][0] += MagCal->vecA[X + 3];
MagCal->matA[0][1] += MagCal->vecA[X] * MagCal->vecA[Y];
MagCal->matA[0][2] += MagCal->vecA[X] * MagCal->vecA[Z];
MagCal->matA[0][3] += MagCal->vecA[X];
MagCal->matA[1][1] += MagCal->vecA[Y + 3];
MagCal->matA[1][2] += MagCal->vecA[Y] * MagCal->vecA[Z];
MagCal->matA[1][3] += MagCal->vecA[Y];
MagCal->matA[2][2] += MagCal->vecA[Z + 3];
MagCal->matA[2][3] += MagCal->vecA[Z];
// increment the counter for next iteration
iCount++;
@ -204,77 +204,77 @@ static void fUpdateCalibration4INV(MagCalibration_t *MagCal)
}
// set the last element of the measurement matrix to the number of buffer elements used
MagCal->fmatA[3][3] = (float) iCount;
MagCal->matA[3][3] = (float) iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
MagCal->MagBufferCount = iCount;
// use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
// use above diagonal elements of symmetric matA to set both matB and matA to X^T.X
for (i = 0; i < 4; i++) {
for (j = i; j < 4; j++) {
MagCal->fmatB[i][j] = MagCal->fmatB[j][i]
= MagCal->fmatA[j][i] = MagCal->fmatA[i][j];
MagCal->matB[i][j] = MagCal->matB[j][i]
= MagCal->matA[j][i] = MagCal->matA[i][j];
}
}
// calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
// calculate in situ inverse of matB = inv(X^T.X) (4x4) while matA still holds X^T.X
for (i = 0; i < 4; i++) {
pfRows[i] = MagCal->fmatB[i];
pfRows[i] = MagCal->matB[i];
}
fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4);
// calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
// calculate vecA = solution beta (4x1) = inv(X^T.X).X^T.Y = matB * vecB
for (i = 0; i < 4; i++) {
MagCal->fvecA[i] = 0.0F;
MagCal->vecA[i] = 0.0F;
for (k = 0; k < 4; k++) {
MagCal->fvecA[i] += MagCal->fmatB[i][k] * MagCal->fvecB[k];
MagCal->vecA[i] += MagCal->matB[i][k] * MagCal->vecB[k];
}
}
// calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
// = fSumBp4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBp4 - 2 * fvecA^T.fvecB
// = fSumBp4 - 2 * vecA^T.vecB + vecA^T.matA.vecA
// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = SumBp4 - 2 * vecA^T.vecB
fE = 0.0F;
for (i = 0; i < 4; i++) {
fE += MagCal->fvecA[i] * MagCal->fvecB[i];
fE += MagCal->vecA[i] * MagCal->vecB[i];
}
fE = fSumBp4 - 2.0F * fE;
// set fvecB = (X^T.X).beta = fmatA.fvecA
// set vecB = (X^T.X).beta = matA.vecA
for (i = 0; i < 4; i++) {
MagCal->fvecB[i] = 0.0F;
MagCal->vecB[i] = 0.0F;
for (k = 0; k < 4; k++) {
MagCal->fvecB[i] += MagCal->fmatA[i][k] * MagCal->fvecA[k];
MagCal->vecB[i] += MagCal->matA[i][k] * MagCal->vecA[k];
}
}
// complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
// complete calculation of P by adding beta^T.(X^T.X).beta = vecA^T * vecB
for (i = 0; i < 4; i++) {
fE += MagCal->fvecB[i] * MagCal->fvecA[i];
fE += MagCal->vecB[i] * MagCal->vecA[i];
}
// compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = 0.5F * MagCal->fvecA[k];
MagCal->trV[k] = 0.5F * MagCal->vecA[k];
}
// compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
MagCal->ftrB = sqrtf(MagCal->fvecA[3] + MagCal->ftrV[X] * MagCal->ftrV[X] +
MagCal->ftrV[Y] * MagCal->ftrV[Y] + MagCal->ftrV[Z] * MagCal->ftrV[Z]);
MagCal->trB = sqrtf(MagCal->vecA[3] + MagCal->trV[X] * MagCal->trV[X] +
MagCal->trV[Y] * MagCal->trV[Y] + MagCal->trV[Z] * MagCal->trV[Z]);
// calculate the trial fit error (percent) normalized to number of measurements
// and scaled geomagnetic field strength
MagCal->ftrFitErrorpc = sqrtf(fE / (float) MagCal->iMagBufferCount) * 100.0F /
(2.0F * MagCal->ftrB * MagCal->ftrB);
MagCal->trFitErrorpc = sqrtf(fE / (float) MagCal->MagBufferCount) * 100.0F /
(2.0F * MagCal->trB * MagCal->trB);
// correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB
MagCal->trV[k] = MagCal->trV[k] * DEFAULTB
+ (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
// correct the geomagnetic field strength B to correct scaling (result in uT)
MagCal->ftrB *= DEFAULTB;
MagCal->trB *= DEFAULTB;
}
@ -302,43 +302,43 @@ static void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
// the offsets are guaranteed to be set from the first element but to avoid compiler error
iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix matA
for (m = 0; m < 7; m++) {
for (n = m; n < 7; n++) {
MagCal->fmatA[m][n] = 0.0F;
MagCal->matA[m][n] = 0.0F;
}
}
// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix fmatA
// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix matA
iCount = 0;
for (j = 0; j < MAGBUFFSIZE; j++) {
if (MagCal->valid[j]) {
// use first valid magnetic buffer entry as offset estimate (bit counts)
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
iOffset[k] = MagCal->BpFast[k][j];
}
}
// apply the offset and scaling and store in fvecA
// apply the offset and scaling and store in vecA
for (k = X; k <= Z; k++) {
MagCal->fvecA[k + 3] = (float)((int32_t)MagCal->iBpFast[k][j]
MagCal->vecA[k + 3] = (float)((int32_t)MagCal->BpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
MagCal->fvecA[k] = MagCal->fvecA[k + 3] * MagCal->fvecA[k + 3];
MagCal->vecA[k] = MagCal->vecA[k + 3] * MagCal->vecA[k + 3];
}
// accumulate the on-and above-diagonal terms of
// MagCal->fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[6][6] which will sum to the number
// of measurements and remembering that fvecA[6] equals 1.0F
// update the right hand column [6] of fmatA except for fmatA[6][6]
// MagCal->matA=Sigma{vecA^T * vecA}
// with the exception of matA[6][6] which will sum to the number
// of measurements and remembering that vecA[6] equals 1.0F
// update the right hand column [6] of matA except for matA[6][6]
for (m = 0; m < 6; m++) {
MagCal->fmatA[m][6] += MagCal->fvecA[m];
MagCal->matA[m][6] += MagCal->vecA[m];
}
// update the on and above diagonal terms except for right hand column 6
for (m = 0; m < 6; m++) {
for (n = m; n < 6; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
}
}
@ -347,71 +347,71 @@ static void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
}
}
// finally set the last element fmatA[6][6] to the number of measurements
MagCal->fmatA[6][6] = (float) iCount;
// finally set the last element matA[6][6] to the number of measurements
MagCal->matA[6][6] = (float) iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
MagCal->MagBufferCount = iCount;
// copy the above diagonal elements of fmatA to below the diagonal
// copy the above diagonal elements of matA to below the diagonal
for (m = 1; m < 7; m++) {
for (n = 0; n < m; n++) {
MagCal->fmatA[m][n] = MagCal->fmatA[n][m];
MagCal->matA[m][n] = MagCal->matA[n][m];
}
}
// set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 7);
// set tmpA7x1 to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 7);
// find the smallest eigenvalue
j = 0;
for (i = 1; i < 7; i++) {
if (MagCal->fvecA[i] < MagCal->fvecA[j]) {
if (MagCal->vecA[i] < MagCal->vecA[j]) {
j = i;
}
}
// set ellipsoid matrix A to the solution vector with smallest eigenvalue,
// compute its determinant and the hard iron offset (scaled and offset)
f3x3matrixAeqScalar(MagCal->fA, 0.0F);
f3x3matrixAeqScalar(MagCal->A, 0.0F);
det = 1.0F;
for (k = X; k <= Z; k++) {
MagCal->fA[k][k] = MagCal->fmatB[k][j];
det *= MagCal->fA[k][k];
MagCal->ftrV[k] = -0.5F * MagCal->fmatB[k + 3][j] / MagCal->fA[k][k];
MagCal->A[k][k] = MagCal->matB[k][j];
det *= MagCal->A[k][k];
MagCal->trV[k] = -0.5F * MagCal->matB[k + 3][j] / MagCal->A[k][k];
}
// negate A if it has negative determinant
if (det < 0.0F) {
f3x3matrixAeqMinusA(MagCal->fA);
MagCal->fmatB[6][j] = -MagCal->fmatB[6][j];
f3x3matrixAeqMinusA(MagCal->A);
MagCal->matB[6][j] = -MagCal->matB[6][j];
det = -det;
}
// set ftmp to the square of the trial geomagnetic field strength B
// (counts times FMATRIXSCALING)
ftmp = -MagCal->fmatB[6][j];
ftmp = -MagCal->matB[6][j];
for (k = X; k <= Z; k++) {
ftmp += MagCal->fA[k][k] * MagCal->ftrV[k] * MagCal->ftrV[k];
ftmp += MagCal->A[k][k] * MagCal->trV[k] * MagCal->trV[k];
}
// calculate the trial normalized fit error as a percentage
MagCal->ftrFitErrorpc = 50.0F *
sqrtf(fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) / fabs(ftmp);
MagCal->trFitErrorpc = 50.0F *
sqrtf(fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) / fabs(ftmp);
// normalize the ellipsoid matrix A to unit determinant
f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));
// convert the geomagnetic field strength B into uT for normalized
// soft iron matrix A and normalize
MagCal->ftrB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
MagCal->trB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
// compute trial invW from the square root of A also with normalized
// determinant and hard iron offset in uT
f3x3matrixAeqI(MagCal->ftrinvW);
f3x3matrixAeqI(MagCal->trinvW);
for (k = X; k <= Z; k++) {
MagCal->ftrinvW[k][k] = sqrtf(fabs(MagCal->fA[k][k]));
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
MagCal->trinvW[k][k] = sqrtf(fabs(MagCal->A[k][k]));
MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
}
@ -435,14 +435,14 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
// the offsets are guaranteed to be set from the first element but to avoid compiler error
iOffset[X] = iOffset[Y] = iOffset[Z] = 0;
// zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
// zero the on and above diagonal elements of the 10x10 symmetric measurement matrix matA
for (m = 0; m < 10; m++) {
for (n = m; n < 10; n++) {
MagCal->fmatA[m][n] = 0.0F;
MagCal->matA[m][n] = 0.0F;
}
}
// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix fmatA
// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix matA
iCount = 0;
for (j = 0; j < MAGBUFFSIZE; j++) {
if (MagCal->valid[j] != -1) {
@ -450,34 +450,34 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
// to help solution (bit counts)
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
iOffset[k] = MagCal->BpFast[k][j];
}
}
// apply the fixed offset and scaling and enter into fvecA[6-8]
// apply the fixed offset and scaling and enter into vecA[6-8]
for (k = X; k <= Z; k++) {
MagCal->fvecA[k + 6] = (float)((int32_t)MagCal->iBpFast[k][j]
MagCal->vecA[k + 6] = (float)((int32_t)MagCal->BpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
}
// compute measurement vector elements fvecA[0-5] from fvecA[6-8]
MagCal->fvecA[0] = MagCal->fvecA[6] * MagCal->fvecA[6];
MagCal->fvecA[1] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[7];
MagCal->fvecA[2] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[8];
MagCal->fvecA[3] = MagCal->fvecA[7] * MagCal->fvecA[7];
MagCal->fvecA[4] = 2.0F * MagCal->fvecA[7] * MagCal->fvecA[8];
MagCal->fvecA[5] = MagCal->fvecA[8] * MagCal->fvecA[8];
// compute measurement vector elements vecA[0-5] from vecA[6-8]
MagCal->vecA[0] = MagCal->vecA[6] * MagCal->vecA[6];
MagCal->vecA[1] = 2.0F * MagCal->vecA[6] * MagCal->vecA[7];
MagCal->vecA[2] = 2.0F * MagCal->vecA[6] * MagCal->vecA[8];
MagCal->vecA[3] = MagCal->vecA[7] * MagCal->vecA[7];
MagCal->vecA[4] = 2.0F * MagCal->vecA[7] * MagCal->vecA[8];
MagCal->vecA[5] = MagCal->vecA[8] * MagCal->vecA[8];
// accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[9][9] which equals the number of measurements
// update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
// accumulate the on-and above-diagonal terms of matA=Sigma{vecA^T * vecA}
// with the exception of matA[9][9] which equals the number of measurements
// update the right hand column [9] of matA[0-8][9] ignoring matA[9][9]
for (m = 0; m < 9; m++) {
MagCal->fmatA[m][9] += MagCal->fvecA[m];
MagCal->matA[m][9] += MagCal->vecA[m];
}
// update the on and above diagonal terms of fmatA ignoring right hand column 9
// update the on and above diagonal terms of matA ignoring right hand column 9
for (m = 0; m < 9; m++) {
for (n = m; n < 9; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
MagCal->matA[m][n] += MagCal->vecA[m] * MagCal->vecA[n];
}
}
@ -486,121 +486,121 @@ static void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
}
}
// set the last element fmatA[9][9] to the number of measurements
MagCal->fmatA[9][9] = (float) iCount;
// set the last element matA[9][9] to the number of measurements
MagCal->matA[9][9] = (float) iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
MagCal->MagBufferCount = iCount;
// copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
// copy the above diagonal elements of symmetric product matrix matA to below the diagonal
for (m = 1; m < 10; m++) {
for (n = 0; n < m; n++) {
MagCal->fmatA[m][n] = MagCal->fmatA[n][m];
MagCal->matA[m][n] = MagCal->matA[n][m];
}
}
// set MagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted
// normalized eigenvectors of fmatA
eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 10);
// set MagCal->vecA to the unsorted eigenvalues and matB to the unsorted
// normalized eigenvectors of matA
eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 10);
// set ellipsoid matrix A from elements of the solution vector column j with
// smallest eigenvalue
j = 0;
for (i = 1; i < 10; i++) {
if (MagCal->fvecA[i] < MagCal->fvecA[j]) {
if (MagCal->vecA[i] < MagCal->vecA[j]) {
j = i;
}
}
MagCal->fA[0][0] = MagCal->fmatB[0][j];
MagCal->fA[0][1] = MagCal->fA[1][0] = MagCal->fmatB[1][j];
MagCal->fA[0][2] = MagCal->fA[2][0] = MagCal->fmatB[2][j];
MagCal->fA[1][1] = MagCal->fmatB[3][j];
MagCal->fA[1][2] = MagCal->fA[2][1] = MagCal->fmatB[4][j];
MagCal->fA[2][2] = MagCal->fmatB[5][j];
MagCal->A[0][0] = MagCal->matB[0][j];
MagCal->A[0][1] = MagCal->A[1][0] = MagCal->matB[1][j];
MagCal->A[0][2] = MagCal->A[2][0] = MagCal->matB[2][j];
MagCal->A[1][1] = MagCal->matB[3][j];
MagCal->A[1][2] = MagCal->A[2][1] = MagCal->matB[4][j];
MagCal->A[2][2] = MagCal->matB[5][j];
// negate entire solution if A has negative determinant
det = f3x3matrixDetA(MagCal->fA);
det = f3x3matrixDetA(MagCal->A);
if (det < 0.0F) {
f3x3matrixAeqMinusA(MagCal->fA);
MagCal->fmatB[6][j] = -MagCal->fmatB[6][j];
MagCal->fmatB[7][j] = -MagCal->fmatB[7][j];
MagCal->fmatB[8][j] = -MagCal->fmatB[8][j];
MagCal->fmatB[9][j] = -MagCal->fmatB[9][j];
f3x3matrixAeqMinusA(MagCal->A);
MagCal->matB[6][j] = -MagCal->matB[6][j];
MagCal->matB[7][j] = -MagCal->matB[7][j];
MagCal->matB[8][j] = -MagCal->matB[8][j];
MagCal->matB[9][j] = -MagCal->matB[9][j];
det = -det;
}
// compute the inverse of the ellipsoid matrix
f3x3matrixAeqInvSymB(MagCal->finvA, MagCal->fA);
f3x3matrixAeqInvSymB(MagCal->invA, MagCal->A);
// compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = 0.0F;
MagCal->trV[k] = 0.0F;
for (m = X; m <= Z; m++) {
MagCal->ftrV[k] += MagCal->finvA[k][m] * MagCal->fmatB[m + 6][j];
MagCal->trV[k] += MagCal->invA[k][m] * MagCal->matB[m + 6][j];
}
MagCal->ftrV[k] *= -0.5F;
MagCal->trV[k] *= -0.5F;
}
// compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
MagCal->ftrB = sqrtf(fabs(MagCal->fA[0][0] * MagCal->ftrV[X] * MagCal->ftrV[X] +
2.0F * MagCal->fA[0][1] * MagCal->ftrV[X] * MagCal->ftrV[Y] +
2.0F * MagCal->fA[0][2] * MagCal->ftrV[X] * MagCal->ftrV[Z] +
MagCal->fA[1][1] * MagCal->ftrV[Y] * MagCal->ftrV[Y] +
2.0F * MagCal->fA[1][2] * MagCal->ftrV[Y] * MagCal->ftrV[Z] +
MagCal->fA[2][2] * MagCal->ftrV[Z] * MagCal->ftrV[Z] - MagCal->fmatB[9][j]));
MagCal->trB = sqrtf(fabs(MagCal->A[0][0] * MagCal->trV[X] * MagCal->trV[X] +
2.0F * MagCal->A[0][1] * MagCal->trV[X] * MagCal->trV[Y] +
2.0F * MagCal->A[0][2] * MagCal->trV[X] * MagCal->trV[Z] +
MagCal->A[1][1] * MagCal->trV[Y] * MagCal->trV[Y] +
2.0F * MagCal->A[1][2] * MagCal->trV[Y] * MagCal->trV[Z] +
MagCal->A[2][2] * MagCal->trV[Z] * MagCal->trV[Z] - MagCal->matB[9][j]));
// calculate the trial normalized fit error as a percentage
MagCal->ftrFitErrorpc = 50.0F * sqrtf(
fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) /
(MagCal->ftrB * MagCal->ftrB);
MagCal->trFitErrorpc = 50.0F * sqrtf(
fabs(MagCal->vecA[j]) / (float) MagCal->MagBufferCount) /
(MagCal->trB * MagCal->trB);
// correct for the measurement matrix offset and scaling and
// get the computed hard iron offset in uT
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
MagCal->trV[k] = MagCal->trV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
// convert the trial geomagnetic field strength B into uT for
// un-normalized soft iron matrix A
MagCal->ftrB *= DEFAULTB;
MagCal->trB *= DEFAULTB;
// normalize the ellipsoid matrix A to unit determinant and
// correct B by root of this multiplicative factor
f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
MagCal->ftrB *= powf(det, -(ONESIXTH));
f3x3matrixAeqAxScalar(MagCal->A, powf(det, -(ONETHIRD)));
MagCal->trB *= powf(det, -(ONESIXTH));
// compute trial invW from the square root of fA (both with normalized determinant)
// set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
// where fmatA holds the 3x3 matrix fA in its top left elements
// compute trial invW from the square root of fA (both with normalized determinant)
// set vecA to the unsorted eigenvalues and matB to the unsorted eigenvectors of matA
// where matA holds the 3x3 matrix fA in its top left elements
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
MagCal->fmatA[i][j] = MagCal->fA[i][j];
MagCal->matA[i][j] = MagCal->A[i][j];
}
}
eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 3);
eigencompute(MagCal->matA, MagCal->vecA, MagCal->matB, 3);
// set MagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
// fmatB . diag(sqrt(sqrt(fvecA))
// set MagCal->matB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
// matB . diag(sqrt(sqrt(vecA))
for (j = 0; j < 3; j++) { // loop over columns j
ftmp = sqrtf(sqrtf(fabs(MagCal->fvecA[j])));
ftmp = sqrtf(sqrtf(fabs(MagCal->vecA[j])));
for (i = 0; i < 3; i++) { // loop over rows i
MagCal->fmatB[i][j] *= ftmp;
MagCal->matB[i][j] *= ftmp;
}
}
// set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
// fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
// set trinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
// matB * matB^T = sqrt(fA) (guaranteed symmetric)
// loop over rows
for (i = 0; i < 3; i++) {
// loop over on and above diagonal columns
for (j = i; j < 3; j++) {
MagCal->ftrinvW[i][j] = 0.0F;
MagCal->trinvW[i][j] = 0.0F;
// accumulate the matrix product
for (k = 0; k < 3; k++) {
MagCal->ftrinvW[i][j] += MagCal->fmatB[i][k] * MagCal->fmatB[j][k];
MagCal->trinvW[i][j] += MagCal->matB[i][k] * MagCal->matB[j][k];
}
// copy to below diagonal element
MagCal->ftrinvW[j][i] = MagCal->ftrinvW[i][j];
MagCal->trinvW[j][i] = MagCal->trinvW[i][j];
}
}
}

4
matrix.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

89
rawdata.c
View file

@ -10,14 +10,14 @@ SV_9DOF_GBY_KALMAN_t fusionstate;
void raw_data_reset(void)
{
rawcount = OVERSAMPLE_RATIO;
fInit_9DOF_GBY_KALMAN(&fusionstate, 100, OVERSAMPLE_RATIO);
fInit_9DOF_GBY_KALMAN(&fusionstate);
memset(&magcal, 0, sizeof(magcal));
magcal.fV[2] = 80.0f; // initial guess
magcal.finvW[0][0] = 1.0f;
magcal.finvW[1][1] = 1.0f;
magcal.finvW[2][2] = 1.0f;
magcal.fFitErrorpc = 100.0f;
magcal.fFitErrorAge = 100.0f;
magcal.V[2] = 80.0f; // initial guess
magcal.invW[0][0] = 1.0f;
magcal.invW[1][1] = 1.0f;
magcal.invW[2][2] = 1.0f;
magcal.FitError = 100.0f;
magcal.FitErrorAge = 100.0f;
}
static void add_magcal_data(const int16_t *data)
@ -36,9 +36,9 @@ static void add_magcal_data(const int16_t *data)
if (i >= MAGBUFFSIZE) {
for (i=0; i < MAGBUFFSIZE; i++) {
for (j=i+1; j < MAGBUFFSIZE; j++) {
dx = magcal.iBpFast[0][i] - magcal.iBpFast[0][j];
dy = magcal.iBpFast[1][i] - magcal.iBpFast[1][j];
dz = magcal.iBpFast[2][i] - magcal.iBpFast[2][j];
dx = magcal.BpFast[0][i] - magcal.BpFast[0][j];
dy = magcal.BpFast[1][i] - magcal.BpFast[1][j];
dz = magcal.BpFast[2][i] - magcal.BpFast[2][j];
distsq = (int64_t)dx * (int64_t)dx;
distsq += (int64_t)dy * (int64_t)dy;
distsq += (int64_t)dz * (int64_t)dz;
@ -51,9 +51,9 @@ static void add_magcal_data(const int16_t *data)
i = minindex;
}
// add it to the cal buffer
magcal.iBpFast[0][i] = data[6];
magcal.iBpFast[1][i] = data[7];
magcal.iBpFast[2][i] = data[8];
magcal.BpFast[0][i] = data[6];
magcal.BpFast[1][i] = data[7];
magcal.BpFast[2][i] = data[8];
magcal.valid[i] = 1;
}
@ -75,47 +75,46 @@ void raw_data(const int16_t *data)
x = (float)data[0] * G_PER_COUNT;
y = (float)data[1] * G_PER_COUNT;
z = (float)data[2] * G_PER_COUNT;
accel.fGpFast[0] = x;
accel.fGpFast[1] = y;
accel.fGpFast[2] = y;
accel.fGp[0] += x;
accel.fGp[1] += y;
accel.fGp[2] += y;
accel.GpFast[0] = x;
accel.GpFast[1] = y;
accel.GpFast[2] = y;
accel.Gp[0] += x;
accel.Gp[1] += y;
accel.Gp[2] += y;
x = (float)data[3] * DEG_PER_SEC_PER_COUNT;
y = (float)data[4] * DEG_PER_SEC_PER_COUNT;
z = (float)data[5] * DEG_PER_SEC_PER_COUNT;
gyro.fYp[0] += x;
gyro.fYp[1] += y;
gyro.fYp[2] += z;
gyro.iYpFast[rawcount][0] = data[3];
gyro.iYpFast[rawcount][1] = data[4];
gyro.iYpFast[rawcount][2] = data[5];
gyro.Yp[0] += x;
gyro.Yp[1] += y;
gyro.Yp[2] += z;
gyro.YpFast[rawcount][0] = data[3];
gyro.YpFast[rawcount][1] = data[4];
gyro.YpFast[rawcount][2] = data[5];
apply_calibration(data[6], data[7], data[8], &point);
mag.fBcFast[0] = point.x;
mag.fBcFast[1] = point.y;
mag.fBcFast[2] = point.z;
mag.fBc[0] += point.x;
mag.fBc[1] += point.y;
mag.fBc[2] += point.z;
mag.BcFast[0] = point.x;
mag.BcFast[1] = point.y;
mag.BcFast[2] = point.z;
mag.Bc[0] += point.x;
mag.Bc[1] += point.y;
mag.Bc[2] += point.z;
rawcount++;
if (rawcount >= OVERSAMPLE_RATIO) {
ratio = 1.0f / (float)OVERSAMPLE_RATIO;
accel.fGp[0] *= ratio;
accel.fGp[1] *= ratio;
accel.fGp[2] *= ratio;
gyro.fYp[0] *= ratio;
gyro.fYp[1] *= ratio;
gyro.fYp[2] *= ratio;
mag.fBc[0] *= ratio;
mag.fBc[1] *= ratio;
mag.fBc[2] *= ratio;
fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro,
&magcal, OVERSAMPLE_RATIO);
accel.Gp[0] *= ratio;
accel.Gp[1] *= ratio;
accel.Gp[2] *= ratio;
gyro.Yp[0] *= ratio;
gyro.Yp[1] *= ratio;
gyro.Yp[2] *= ratio;
mag.Bc[0] *= ratio;
mag.Bc[1] *= ratio;
mag.Bc[2] *= ratio;
fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro, &magcal);
memcpy(&current_orientation, &(fusionstate.fqPl), sizeof(Quaternion_t));
memcpy(&current_orientation, &(fusionstate.qPl), sizeof(Quaternion_t));
}
}
@ -159,11 +158,11 @@ int send_calibration(void)
*p++ = 117; // 2 byte signature
*p++ = 84;
for (i=0; i < 3; i++) {
p = copy_lsb_first(p, magcal.fV[i]); // 12 bytes offset/hardiron
p = copy_lsb_first(p, magcal.V[i]); // 12 bytes offset/hardiron
}
for (i=0; i < 3; i++) {
for (j=0; j < 3; j++) {
p = copy_lsb_first(p, magcal.finvW[i][j]); // 36 bytes softiron
p = copy_lsb_first(p, magcal.invW[i][j]); // 36 bytes softiron
}
}
crc = 0xFFFF;

34
serialdata.c
View file

@ -50,30 +50,30 @@ static int packet_magnetic_cal(const unsigned char *data)
z = (data[13] << 8) | data[12];
if (id == 1) {
magcal.fV[0] = (float)x * 0.1f;
magcal.fV[1] = (float)y * 0.1f;
magcal.fV[2] = (float)z * 0.1f;
magcal.V[0] = (float)x * 0.1f;
magcal.V[1] = (float)y * 0.1f;
magcal.V[2] = (float)z * 0.1f;
return 1;
} else if (id == 2) {
magcal.finvW[0][0] = (float)x * 0.001f;
magcal.finvW[1][1] = (float)y * 0.001f;
magcal.finvW[2][2] = (float)z * 0.001f;
magcal.invW[0][0] = (float)x * 0.001f;
magcal.invW[1][1] = (float)y * 0.001f;
magcal.invW[2][2] = (float)z * 0.001f;
return 1;
} else if (id == 3) {
magcal.finvW[0][1] = (float)x / 1000.0f;
magcal.finvW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
magcal.finvW[0][2] = (float)y / 1000.0f;
magcal.finvW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
magcal.finvW[1][2] = (float)z / 1000.0f;
magcal.finvW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
magcal.invW[0][1] = (float)x / 1000.0f;
magcal.invW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
magcal.invW[0][2] = (float)y / 1000.0f;
magcal.invW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
magcal.invW[1][2] = (float)z / 1000.0f;
magcal.invW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
return 1;
} else if (id >= 10 && id < MAGBUFFSIZE+10) {
n = id - 10;
if (magcal.valid[n] == 0 || x != magcal.iBpFast[0][n]
|| y != magcal.iBpFast[1][n] || z != magcal.iBpFast[2][n]) {
magcal.iBpFast[0][n] = x;
magcal.iBpFast[1][n] = y;
magcal.iBpFast[2][n] = z;
if (magcal.valid[n] == 0 || x != magcal.BpFast[0][n]
|| y != magcal.BpFast[1][n] || z != magcal.BpFast[2][n]) {
magcal.BpFast[0][n] = x;
magcal.BpFast[1][n] = y;
magcal.BpFast[2][n] = z;
magcal.valid[n] = 1;
printf("mag cal, n=%3d: %5d %5d %5d\n", n, x, y, z);
}

16
visualize.c
View file

@ -50,12 +50,12 @@ void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
{
float x, y, z;
x = ((float)rawx * UT_PER_COUNT) - magcal.fV[0];
y = ((float)rawy * UT_PER_COUNT) - magcal.fV[1];
z = ((float)rawz * UT_PER_COUNT) - magcal.fV[2];
out->x = x * magcal.finvW[0][0] + y * magcal.finvW[0][1] + z * magcal.finvW[0][2];
out->y = x * magcal.finvW[1][0] + y * magcal.finvW[1][1] + z * magcal.finvW[1][2];
out->z = x * magcal.finvW[2][0] + y * magcal.finvW[2][1] + z * magcal.finvW[2][2];
x = ((float)rawx * UT_PER_COUNT) - magcal.V[0];
y = ((float)rawy * UT_PER_COUNT) - magcal.V[1];
z = ((float)rawz * UT_PER_COUNT) - magcal.V[2];
out->x = x * magcal.invW[0][0] + y * magcal.invW[0][1] + z * magcal.invW[0][2];
out->y = x * magcal.invW[1][0] + y * magcal.invW[1][1] + z * magcal.invW[1][2];
out->z = x * magcal.invW[2][0] + y * magcal.invW[2][1] + z * magcal.invW[2][2];
}
static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
@ -119,8 +119,8 @@ void display_callback(void)
//if (caldata[i].valid) {
if (magcal.valid[i]) {
//apply_calibration(&caldata[i], &point);
apply_calibration(magcal.iBpFast[0][i], magcal.iBpFast[1][i],
magcal.iBpFast[2][i], &point);
apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i],
magcal.BpFast[2][i], &point);
sumx += point.x;
sumy += point.y;
sumz += point.z;