// Fuse the SmallEKF velocity estimates - this enables alevel reference to be maintained during constant turns
void SmallEKF::fuseVelocity(bool yawInit)
{
float R_OBS = 0.25f;
float innovation[3];
float varInnov[3];
Vector3f angErrVec;
uint8_t stateIndex;
float K[9];
// Fuse measurements sequentially
for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
stateIndex = 3 + obsIndex;
// Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
// if heading isn't aligned, use zero velocity (static assumption)
if (yawInit) {
Vector3f measVelNED;
_main_ekf.getVelNED(measVelNED);
innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
} else {
innovation[obsIndex] = state.velocity[obsIndex];
}
// Zero the attitude error states - they represent the incremental error so must be zero before corrections are applied
state.angErr.zero();
// Calculate the innovation variance
varInnov[obsIndex] = Cov[stateIndex][stateIndex] + R_OBS;
// Calculate the Kalman gain and correct states, taking advantage of direct state observation
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
K[rowIndex] = Cov[rowIndex][stateIndex]/varInnov[obsIndex];
states[rowIndex] -= K[rowIndex] * innovation[obsIndex];
}
// Store tilt error estimate for external monitoring
angErrVec = angErrVec + state.angErr;
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion
// Convert the error rotation vector to its equivalent quaternion
// truth = estimate + error
float rotationMag = state.angErr.length();
if (rotationMag > 1e-6f) {
Quaternion deltaQuat;
float temp = sinf(0.5f*rotationMag) / rotationMag;
deltaQuat[0] = cosf(0.5f*rotationMag);
deltaQuat[1] = state.angErr.x*temp;
deltaQuat[2] = state.angErr.y*temp;
deltaQuat[3] = state.angErr.z*temp;
// Update the quaternion states by rotating from the previous attitude through the error quaternion
state.quat *= deltaQuat;
// re-normalise the quaternion
state.quat.normalize();
}
// Update the covariance
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
Cov[rowIndex][colIndex] = Cov[rowIndex][colIndex] - K[rowIndex]*Cov[stateIndex][colIndex];
}
}
// force symmetry and constrain diagonals to be non-negative
fixCovariance();
}
// calculate tilt component of angle correction
TiltCorrection = sqrtf(sq(angErrVec.x) + sq(angErrVec.y));
}
// Fuse compass measurements from autopilot
void SmallEKF::fuseCompass()
{
float q0 = state.quat[0];
float q1 = state.quat[1];
float q2 = state.quat[2];
float q3 = state.quat[3];
float magX = magData.x;
float magY = magData.y;
float magZ = magData.z;
const float R_MAG = 3e-2f;
// Calculate observation Jacobian
float t5695 = sq(q0);
float t5696 = sq(q1);
float t5697 = sq(q2);
float t5698 = sq(q3);
float t5699 = t5695+t5696-t5697-t5698;
float t5702 = q0*q2*2.0f;
float t5703 = q1*q3*2.0f;
float t5704 = t5702+t5703;
float t5705 = q0*q3*2.0f;
float t5707 = q1*q2*2.0f;
float t5706 = t5705-t5707;
float t5708 = cosTheta*sinPsi;
float t5709 = sinPhi*sinTheta*cosPsi;
float t5710 = t5708+t5709;
float t5711 = t5705+t5707;
float t5712 = sinTheta*sinPsi;
float t5730 = cosTheta*sinPhi*cosPsi;
float t5713 = t5712-t5730;
float t5714 = q0*q1*2.0f;
float t5720 = q2*q3*2.0f;
float t5715 = t5714-t5720;
float t5716 = t5695-t5696+t5697-t5698;
float t5717 = sinTheta*cosPsi;
float t5718 = cosTheta*sinPhi*sinPsi;
float t5719 = t5717+t5718;
float t5721 = cosTheta*cosPsi;
float t5735 = sinPhi*sinTheta*sinPsi;
float t5722 = t5721-t5735;
float t5724 = sinPhi*t5706;
float t5725 = cosPhi*sinTheta*t5699;
float t5726 = cosPhi*cosTheta*t5704;
float t5727 = t5724+t5725-t5726;
float t5728 = magZ*t5727;
float t5729 = t5699*t5710;
float t5731 = t5704*t5713;
float t5732 = cosPhi*cosPsi*t5706;
float t5733 = t5729+t5731-t5732;
float t5734 = magY*t5733;
float t5736 = t5699*t5722;
float t5737 = t5704*t5719;
float t5738 = cosPhi*sinPsi*t5706;
float t5739 = t5736+t5737+t5738;
float t5740 = magX*t5739;
float t5741 = -t5728+t5734+t5740;
float t5742 = 1.0f/t5741;
float t5743 = sinPhi*t5716;
float t5744 = cosPhi*cosTheta*t5715;
float t5745 = cosPhi*sinTheta*t5711;
float t5746 = -t5743+t5744+t5745;
float t5747 = magZ*t5746;
float t5748 = t5710*t5711;
float t5749 = t5713*t5715;
float t5750 = cosPhi*cosPsi*t5716;
float t5751 = t5748-t5749+t5750;
float t5752 = magY*t5751;
float t5753 = t5715*t5719;
float t5754 = t5711*t5722;
float t5755 = cosPhi*sinPsi*t5716;
float t5756 = t5753-t5754+t5755;
float t5757 = magX*t5756;
float t5758 = t5747-t5752+t5757;
float t5759 = t5742*t5758;
float t5723 = tan(t5759);
float t5760 = sq(t5723);
float t5761 = t5760+1.0f;
float t5762 = 1.0f/sq(t5741);
float H_MAG[3];
H_MAG[0] = -t5761*(t5742*(magZ*(sinPhi*t5715+cosPhi*cosTheta*t5716)+magY*(t5713*t5716+cosPhi*cosPsi*t5715)+magX*(t5716*t5719-cosPhi*sinPsi*t5715))-t5758*t5762*(magZ*(sinPhi*t5704+cosPhi*cosTheta*t5706)+magY*(t5706*t5713+cosPhi*cosPsi*t5704)+magX*(t5706*t5719-cosPhi*sinPsi*t5704)));
H_MAG[1] = t5761*(t5742*(magZ*(cosPhi*cosTheta*t5711-cosPhi*sinTheta*t5715)+magY*(t5711*t5713+t5710*t5715)+magX*(t5711*t5719+t5715*t5722))+t5758*t5762*(magZ*(cosPhi*cosTheta*t5699+cosPhi*sinTheta*t5704)+magY*(t5699*t5713-t5704*t5710)+magX*(t5699*t5719-t5704*t5722)));
H_MAG[2] = t5761*(t5742*(-magZ*(sinPhi*t5711+cosPhi*sinTheta*t5716)+magY*(t5710*t5716-cosPhi*cosPsi*t5711)+magX*(t5716*t5722+cosPhi*sinPsi*t5711))-t5758*t5762*(magZ*(sinPhi*t5699-cosPhi*sinTheta*t5706)+magY*(t5706*t5710+cosPhi*t5699*cosPsi)+magX*(t5706*t5722-cosPhi*t5699*sinPsi)));
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
float PH[3];
float varInnov = R_MAG;
for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
PH[rowIndex] = 0.0f;
for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
PH[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
}
varInnov += H_MAG[rowIndex]*PH[rowIndex];
}
float K_MAG[9];
float varInnovInv = 1.0f / varInnov;
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
K_MAG[rowIndex] = 0.0f;
for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
K_MAG[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
}
K_MAG[rowIndex] *= varInnovInv;
}
// Calculate the innovation
float innovation = calcMagHeadingInnov();
// limit the innovation so that initial corrections are not too large
if (innovation > 0.5f) {
innovation = 0.5f;
} else if (innovation < -0.5f) {
innovation = -0.5f;
}
// correct the state vector
state.angErr.zero();
for (uint8_t i=0;i<=8;i++) {
states[i] -= K_MAG[i] * innovation;
}
// the first 3 states represent the angular error vector where truth = estimate + error. This is is used to correct the estimated quaternion
float rotationMag = state.angErr.length();
if (rotationMag > 1e-6f) {
// Convert the error rotation vector to its equivalent quaternion
Quaternion deltaQuat;
float temp = sinf(0.5f*rotationMag) / rotationMag;
deltaQuat[0] = cosf(0.5f*rotationMag);
deltaQuat[1] = state.angErr.x*temp;
deltaQuat[2] = state.angErr.y*temp;
deltaQuat[3] = state.angErr.z*temp;
// Bring the quaternion state estimate back to 'truth' by adding the error
state.quat *= deltaQuat;
// re-normalise the quaternion
state.quat.normalize();
}
// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
float HP[9];
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
HP[colIndex] = 0.0f;
for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
HP[colIndex] += H_MAG[rowIndex]*Cov[rowIndex][colIndex];
}
}
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
Cov[rowIndex][colIndex] -= K_MAG[rowIndex] * HP[colIndex];
}
}
// force symmetry and constrain diagonals to be non-negative
fixCovariance();
}
// Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
void SoloGimbalEKF::fuseVelocity()
{
if (!_ahrs.have_inertial_nav()) {
return;
}
float R_OBS = 0.25f;
float innovation[3];
float varInnov[3];
Vector3f angErrVec;
uint8_t stateIndex;
float K[9];
// Fuse measurements sequentially
for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
stateIndex = 3 + obsIndex;
// Calculate the velocity measurement innovation using the SoloGimbalEKF estimate as the observation
// if heading isn't aligned, use zero velocity (static assumption)
if (YawAligned) {
Vector3f measVelNED = Vector3f(0,0,0);
nav_filter_status main_ekf_status;
if (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.horiz_vel) {
_ahrs.get_velocity_NED(measVelNED);
}
}
innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
} else {
innovation[obsIndex] = state.velocity[obsIndex];
}
// Zero the attitude error states - they represent the incremental error so must be zero before corrections are applied
state.angErr.zero();
// Calculate the innovation variance
varInnov[obsIndex] = Cov[stateIndex][stateIndex] + R_OBS;
// Calculate the Kalman gain and correct states, taking advantage of direct state observation
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
K[rowIndex] = Cov[rowIndex][stateIndex]/varInnov[obsIndex];
states[rowIndex] -= K[rowIndex] * innovation[obsIndex];
}
// Store tilt error estimate for external monitoring
angErrVec = angErrVec + state.angErr;
// the first 3 states represent the angular error vector where truth = estimate + error. This is is used to correct the estimated quaternion
// Bring the quaternion state estimate back to 'truth' by adding the error
state.quat.rotate(state.angErr);
// re-normalise the quaternion
state.quat.normalize();
// Update the covariance
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
Cov[rowIndex][colIndex] = Cov[rowIndex][colIndex] - K[rowIndex]*Cov[stateIndex][colIndex];
}
}
// force symmetry and constrain diagonals to be non-negative
fixCovariance();
}
// calculate tilt component of angle correction
TiltCorrection = sqrtf(sq(angErrVec.x) + sq(angErrVec.y));
}