// Perform an initial heading alignment using the magnetic field and assumed declination void SmallEKF::alignHeading() { // calculate the correction rotation vector in NED frame Vector3f deltaRotNED; deltaRotNED.z = -calcMagHeadingInnov(); // rotate into sensor frame Vector3f angleCorrection = Tsn.transposed()*deltaRotNED; // apply the correction to the quaternion state float rotationMag = deltaRotNED.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] = angleCorrection.x*temp; deltaQuat[2] = angleCorrection.y*temp; deltaQuat[3] = angleCorrection.z*temp; // Bring the quaternion state estimate back to 'truth' by adding the error state.quat *= deltaQuat; // re-normalise the quaternion state.quat.normalize(); } }
// Perform an initial heading alignment using the magnetic field and assumed declination void SoloGimbalEKF::alignHeading() { // calculate the correction rotation vector in NED frame Vector3f deltaRotNED = Vector3f(0,0,-calcMagHeadingInnov()); // rotate into sensor frame Vector3f angleCorrection = Tsn.transposed()*deltaRotNED; // apply the correction to the quaternion state // Bring the quaternion state estimate back to 'truth' by adding the error state.quat.rotate(angleCorrection); // re-normalize the quaternion state.quat.normalize(); }
// 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(); }