4972 lines
247 KiB
C++
4972 lines
247 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||
|
||
#include <AP_HAL/AP_HAL.h>
|
||
|
||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||
|
||
/*
|
||
optionally turn down optimisation for debugging
|
||
*/
|
||
// #pragma GCC optimize("O0")
|
||
|
||
#include "AP_NavEKF2.h"
|
||
#include "AP_NavEKF2_core.h"
|
||
#include <AP_AHRS/AP_AHRS.h>
|
||
#include <AP_Vehicle/AP_Vehicle.h>
|
||
|
||
#include <stdio.h>
|
||
|
||
extern const AP_HAL::HAL& hal;
|
||
|
||
#define earthRate 0.000072921f // earth rotation rate (rad/sec)
|
||
|
||
// when the wind estimation first starts with no airspeed sensor,
|
||
// assume 3m/s to start
|
||
#define STARTUP_WIND_SPEED 3.0f
|
||
|
||
// initial imu bias uncertainty (deg/sec)
|
||
#define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f
|
||
|
||
// constructor
|
||
NavEKF2_core::NavEKF2_core(NavEKF2 &_frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
|
||
frontend(_frontend),
|
||
_ahrs(ahrs),
|
||
_baro(baro),
|
||
_rng(rng),
|
||
stateStruct(*reinterpret_cast<struct state_elements *>(&statesArray)),
|
||
|
||
//variables
|
||
lastRngMeasTime_ms(0), // time in msec that the last range measurement was taken
|
||
rngMeasIndex(0) // index into ringbuffer of current range measurement
|
||
|
||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
|
||
_perf_CovariancePrediction(perf_alloc(PC_ELAPSED, "EKF_CovariancePrediction")),
|
||
_perf_FuseVelPosNED(perf_alloc(PC_ELAPSED, "EKF_FuseVelPosNED")),
|
||
_perf_FuseMagnetometer(perf_alloc(PC_ELAPSED, "EKF_FuseMagnetometer")),
|
||
_perf_FuseAirspeed(perf_alloc(PC_ELAPSED, "EKF_FuseAirspeed")),
|
||
_perf_FuseSideslip(perf_alloc(PC_ELAPSED, "EKF_FuseSideslip"))
|
||
#endif
|
||
{
|
||
}
|
||
|
||
// Check basic filter health metrics and return a consolidated health status
|
||
bool NavEKF2_core::healthy(void) const
|
||
{
|
||
uint8_t faultInt;
|
||
getFilterFaults(faultInt);
|
||
if (faultInt > 0) {
|
||
return false;
|
||
}
|
||
if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
|
||
// all three metrics being above 1 means the filter is
|
||
// extremely unhealthy.
|
||
return false;
|
||
}
|
||
// Give the filter a second to settle before use
|
||
if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
|
||
return false;
|
||
}
|
||
// barometer and position innovations must be within limits when on-ground
|
||
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
|
||
if (!filterArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
|
||
return false;
|
||
}
|
||
|
||
// all OK
|
||
return true;
|
||
}
|
||
|
||
// resets position states to last GPS measurement or to zero if in constant position mode
|
||
void NavEKF2_core::ResetPosition(void)
|
||
{
|
||
if (constPosMode || (PV_AidingMode != AID_ABSOLUTE)) {
|
||
stateStruct.position.x = 0;
|
||
stateStruct.position.y = 0;
|
||
} else if (!gpsNotAvailable) {
|
||
// write to state vector and compensate for offset between last GPs measurement and the EKF time horizon
|
||
stateStruct.position.x = gpsDataNew.pos.x + gpsPosGlitchOffsetNE.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
|
||
stateStruct.position.y = gpsDataNew.pos.y + gpsPosGlitchOffsetNE.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
|
||
}
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedOutput[i].position.x = stateStruct.position.x;
|
||
storedOutput[i].position.y = stateStruct.position.y;
|
||
}
|
||
outputDataNew.position.x = stateStruct.position.x;
|
||
outputDataNew.position.y = stateStruct.position.y;
|
||
outputDataDelayed.position.x = stateStruct.position.x;
|
||
outputDataDelayed.position.y = stateStruct.position.y;
|
||
}
|
||
|
||
// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
|
||
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
|
||
void NavEKF2_core::ResetVelocity(void)
|
||
{
|
||
if (constPosMode || PV_AidingMode != AID_ABSOLUTE) {
|
||
stateStruct.velocity.zero();
|
||
} else if (!gpsNotAvailable) {
|
||
// reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero.
|
||
stateStruct.velocity.x = gpsDataNew.vel.x + gpsVelGlitchOffset.x; // north velocity from blended accel data
|
||
stateStruct.velocity.y = gpsDataNew.vel.y + gpsVelGlitchOffset.y; // east velocity from blended accel data
|
||
}
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedOutput[i].velocity.x = stateStruct.velocity.x;
|
||
storedOutput[i].velocity.y = stateStruct.velocity.y;
|
||
}
|
||
outputDataNew.velocity.x = stateStruct.velocity.x;
|
||
outputDataNew.velocity.y = stateStruct.velocity.y;
|
||
outputDataDelayed.velocity.x = stateStruct.velocity.x;
|
||
outputDataDelayed.velocity.y = stateStruct.velocity.y;
|
||
}
|
||
|
||
// reset the vertical position state using the last height measurement
|
||
void NavEKF2_core::ResetHeight(void)
|
||
{
|
||
// read the altimeter
|
||
readHgtData();
|
||
// write to the state vector
|
||
stateStruct.position.z = -baroDataNew.hgt; // down position from blended accel data
|
||
terrainState = stateStruct.position.z + rngOnGnd;
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedOutput[i].position.z = stateStruct.position.z;
|
||
}
|
||
outputDataNew.position.z = stateStruct.position.z;
|
||
outputDataDelayed.position.z = stateStruct.position.z;
|
||
}
|
||
|
||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||
// This method can only be used when the vehicle is static
|
||
bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||
{
|
||
// If we are a plane and don't have GPS lock then don't initialise
|
||
if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) {
|
||
statesInitialised = false;
|
||
return false;
|
||
}
|
||
|
||
// set re-used variables to zero
|
||
InitialiseVariables();
|
||
|
||
// Initialise IMU data
|
||
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
|
||
readIMUData();
|
||
StoreIMU_reset();
|
||
|
||
// acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||
Vector3f initAccVec;
|
||
|
||
// TODO we should average accel readings over several cycles
|
||
initAccVec = _ahrs->get_ins().get_accel();
|
||
|
||
// read the magnetometer data
|
||
readMagData();
|
||
|
||
// normalise the acceleration vector
|
||
float pitch=0, roll=0;
|
||
if (initAccVec.length() > 0.001f) {
|
||
initAccVec.normalize();
|
||
|
||
// calculate initial pitch angle
|
||
pitch = asinf(initAccVec.x);
|
||
|
||
// calculate initial roll angle
|
||
roll = -asinf(initAccVec.y / cosf(pitch));
|
||
}
|
||
|
||
// calculate initial roll and pitch orientation
|
||
stateStruct.quat.from_euler(roll, pitch, 0.0f);
|
||
|
||
// initialise static process state model states
|
||
stateStruct.gyro_bias.zero();
|
||
stateStruct.gyro_scale.x = 1.0f;
|
||
stateStruct.gyro_scale.y = 1.0f;
|
||
stateStruct.gyro_scale.z = 1.0f;
|
||
stateStruct.accel_zbias = 0.0f;
|
||
stateStruct.wind_vel.zero();
|
||
|
||
// read the GPS and set the position and velocity states
|
||
readGpsData();
|
||
ResetVelocity();
|
||
ResetPosition();
|
||
|
||
// read the barometer and set the height state
|
||
readHgtData();
|
||
ResetHeight();
|
||
|
||
// define Earth rotation vector in the NED navigation frame
|
||
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
|
||
|
||
// initialise the covariance matrix
|
||
CovarianceInit();
|
||
|
||
// reset output states
|
||
StoreOutputReset();
|
||
|
||
// set to true now that states have be initialised
|
||
statesInitialised = true;
|
||
|
||
return true;
|
||
}
|
||
|
||
// Update Filter States - this should be called whenever new IMU data is available
|
||
void NavEKF2_core::UpdateFilter()
|
||
{
|
||
// zero the delta quaternion used by the strapdown navigation because it is published
|
||
// and we need to return a zero rotation of the INS fails to update it
|
||
correctedDelAngQuat.initialise();
|
||
|
||
// don't run filter updates if states have not been initialised
|
||
if (!statesInitialised) {
|
||
return;
|
||
}
|
||
|
||
// start the timer used for load measurement
|
||
perf_begin(_perf_UpdateFilter);
|
||
|
||
// TODO - in-flight restart method
|
||
|
||
//get starting time for update step
|
||
imuSampleTime_ms = hal.scheduler->millis();
|
||
|
||
// read IMU data and convert to delta angles and velocities
|
||
readIMUData();
|
||
|
||
// check if on ground
|
||
SetFlightAndFusionModes();
|
||
|
||
// Check arm status and perform required checks and mode changes
|
||
performArmingChecks();
|
||
|
||
// run the strapdown INS equations every IMU update
|
||
UpdateStrapdownEquationsNED();
|
||
|
||
// sum delta angles and time used by covariance prediction
|
||
summedDelAng = summedDelAng + correctedDelAng;
|
||
summedDelVel = summedDelVel + correctedDelVel;
|
||
dt += imuDataDelayed.delAngDT;
|
||
|
||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||
// or the time limit will be exceeded at the next IMU update
|
||
if (((dt >= (frontend.covTimeStepMax - dtIMUavg)) || (summedDelAng.length() > frontend.covDelAngMax))) {
|
||
CovariancePrediction();
|
||
} else {
|
||
covPredStep = false;
|
||
}
|
||
|
||
// Read range finder data which is used by both position and optical flow fusion
|
||
readRangeFinder();
|
||
|
||
// Update states using GPS and altimeter data
|
||
SelectVelPosFusion();
|
||
|
||
// Update states using optical flow data
|
||
SelectFlowFusion();
|
||
|
||
// Check for tilt convergence
|
||
float alpha = 1.0f*dtIMUavg;
|
||
float temp=tiltErrVec.length();
|
||
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
|
||
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
|
||
tiltAlignComplete = true;
|
||
hal.console->printf("EKF tilt alignment complete\n");
|
||
}
|
||
|
||
// once tilt has converged, align yaw using magnetic field measurements
|
||
if (tiltAlignComplete && !yawAlignComplete) {
|
||
Vector3f eulerAngles;
|
||
getEulerAngles(eulerAngles);
|
||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||
StoreQuatReset();
|
||
yawAlignComplete = true;
|
||
hal.console->printf("EKF yaw alignment complete\n");
|
||
}
|
||
|
||
// Update states using magnetometer data
|
||
SelectMagFusion();
|
||
|
||
// Update states using airspeed data
|
||
SelectTasFusion();
|
||
|
||
// Update states using sideslip constraint assumption for fly-forward vehicles
|
||
SelectBetaFusion();
|
||
|
||
// Wind output forward from the fusion to output time horizon
|
||
calcOutputStatesFast();
|
||
|
||
// stop the timer used for load measurement
|
||
perf_end(_perf_UpdateFilter);
|
||
}
|
||
|
||
// select fusion of velocity, position and height measurements
|
||
void NavEKF2_core::SelectVelPosFusion()
|
||
{
|
||
// check for and read new GPS data
|
||
readGpsData();
|
||
|
||
if (RecallGPS() && PV_AidingMode != AID_RELATIVE) {
|
||
fuseVelData = true;
|
||
fusePosData = true;
|
||
// If a long time since last GPS update, then reset position and velocity and reset stored state history
|
||
if ((imuSampleTime_ms - secondLastGpsTime_ms > 5000) && (PV_AidingMode == AID_ABSOLUTE)) {
|
||
// Apply an offset to the GPS position so that the position can be corrected gradually
|
||
gpsPosGlitchOffsetNE.x = stateStruct.position.x - gpsDataDelayed.pos.x;
|
||
gpsPosGlitchOffsetNE.y = stateStruct.position.y - gpsDataDelayed.pos.y;
|
||
// limit the radius of the offset to 100m and decay the offset to zero radially
|
||
decayGpsOffset();
|
||
ResetPosition();
|
||
ResetVelocity();
|
||
CovarianceInit();
|
||
// record the fail time
|
||
lastPosFailTime_ms = imuSampleTime_ms;
|
||
// Reset the normalised innovation to avoid false failing the bad position fusion test
|
||
posTestRatio = 0.0f;
|
||
}
|
||
} else {
|
||
fuseVelData = false;
|
||
fusePosData = false;
|
||
}
|
||
|
||
// check for and read new height data
|
||
readHgtData();
|
||
|
||
// If we haven't received height data for a while, then declare the height data as being timed out
|
||
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0_ms : frontend.hgtRetryTimeMode12_ms;
|
||
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms) {
|
||
hgtTimeout = true;
|
||
}
|
||
|
||
// command fusion of height data
|
||
// wait until the EKF time horizon catches up with the measurement
|
||
if (RecallBaro()) {
|
||
// enable fusion
|
||
fuseHgtData = true;
|
||
}
|
||
|
||
// perform fusion
|
||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||
// ensure that the covariance prediction is up to date before fusing data
|
||
if (!covPredStep) CovariancePrediction();
|
||
FuseVelPosNED();
|
||
}
|
||
|
||
// Detect and declare bad GPS aiding status for minimum 10 seconds if a GPS rejection occurs after
|
||
// rejection of GPS and reset to GPS position. This addresses failure case where errors cause ongoing rejection
|
||
// of GPS and severe loss of position accuracy.
|
||
uint32_t gpsRetryTime;
|
||
if (useAirspeed()) {
|
||
gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
|
||
} else {
|
||
gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
|
||
}
|
||
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime_ms) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime_ms) > gpsRetryTime/2) && fusePosData) {
|
||
lastGpsAidBadTime_ms = imuSampleTime_ms;
|
||
gpsAidingBad = true;
|
||
}
|
||
gpsAidingBad = gpsAidingBad && ((imuSampleTime_ms - lastGpsAidBadTime_ms) < 10000);
|
||
}
|
||
|
||
// select fusion of magnetometer data
|
||
void NavEKF2_core::SelectMagFusion()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_FuseMagnetometer);
|
||
|
||
// check for and read new magnetometer measurements
|
||
readMagData();
|
||
|
||
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
|
||
if (magHealth) {
|
||
magTimeout = false;
|
||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend.magFailTimeLimit_ms && use_compass()) {
|
||
magTimeout = true;
|
||
}
|
||
|
||
bool temp = RecallMag();
|
||
|
||
// determine if conditions are right to start a new fusion cycle
|
||
// wait until the EKF time horizon catches up with the measurement
|
||
bool dataReady = (temp && statesInitialised && use_compass() && yawAlignComplete);
|
||
if (dataReady) {
|
||
// ensure that the covariance prediction is up to date before fusing data
|
||
if (!covPredStep) CovariancePrediction();
|
||
// If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading
|
||
if(inhibitMagStates) {
|
||
fuseCompass();
|
||
magHealth = true;
|
||
magTimeout = false;
|
||
} else {
|
||
// fuse the three magnetometer componenents sequentially
|
||
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) FuseMagnetometer();
|
||
}
|
||
}
|
||
|
||
// stop performance timer
|
||
perf_end(_perf_FuseMagnetometer);
|
||
}
|
||
|
||
// select fusion of true airspeed measurements
|
||
void NavEKF2_core::SelectTasFusion()
|
||
{
|
||
// get true airspeed measurement
|
||
readAirSpdData();
|
||
|
||
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
|
||
if (imuSampleTime_ms - tasDataNew.time_ms > frontend.tasRetryTime_ms) {
|
||
tasTimeout = true;
|
||
}
|
||
|
||
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
|
||
tasDataWaiting = (statesInitialised && !inhibitWindStates && newDataTas);
|
||
if (tasDataWaiting)
|
||
{
|
||
// ensure that the covariance prediction is up to date before fusing data
|
||
if (!covPredStep) CovariancePrediction();
|
||
FuseAirspeed();
|
||
prevTasStep_ms = imuSampleTime_ms;
|
||
tasDataWaiting = false;
|
||
newDataTas = false;
|
||
}
|
||
}
|
||
|
||
// select fusion of synthetic sideslip measurements
|
||
// synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
|
||
// it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc)
|
||
void NavEKF2_core::SelectBetaFusion()
|
||
{
|
||
// set true when the fusion time interval has triggered
|
||
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend.betaAvg_ms);
|
||
// set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
|
||
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend.gpsRetryTimeNoTAS_ms));
|
||
// set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
|
||
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
|
||
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
|
||
if (f_feasible && f_required && f_timeTrigger) {
|
||
// ensure that the covariance prediction is up to date before fusing data
|
||
if (!covPredStep) CovariancePrediction();
|
||
FuseSideslip();
|
||
prevBetaStep_ms = imuSampleTime_ms;
|
||
}
|
||
}
|
||
|
||
// select fusion of optical flow measurements
|
||
void NavEKF2_core::SelectFlowFusion()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_FuseOptFlow);
|
||
// Perform Data Checks
|
||
// Check if the optical flow data is still valid
|
||
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
|
||
// Check if the optical flow sensor has timed out
|
||
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
|
||
// Check if the fusion has timed out (flow measurements have been rejected for too long)
|
||
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
|
||
// check is the terrain offset estimate is still valid
|
||
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
|
||
// Perform tilt check
|
||
bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
|
||
// Constrain measurements to zero if we are using optical flow and are on the ground
|
||
if (frontend._fusionModeGPS == 3 && !takeOffDetected && filterArmed) {
|
||
ofDataDelayed.flowRadXYcomp.zero();
|
||
ofDataDelayed.flowRadXY.zero();
|
||
flowDataValid = true;
|
||
}
|
||
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
|
||
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
|
||
constVelMode = false; // always clear constant velocity mode if constant velocity mode is active
|
||
constPosMode = true;
|
||
PV_AidingMode = AID_NONE;
|
||
// reset the velocity
|
||
ResetVelocity();
|
||
// store the current position to be used to keep reporting the last known position
|
||
lastKnownPositionNE.x = stateStruct.position.x;
|
||
lastKnownPositionNE.y = stateStruct.position.y;
|
||
// reset the position
|
||
ResetPosition();
|
||
}
|
||
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
|
||
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
|
||
if ((newDataFlow || newDataRng) && tiltOK) {
|
||
// fuse range data into the terrain estimator if available
|
||
fuseRngData = newDataRng;
|
||
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
|
||
fuseOptFlowData = (newDataFlow && !fuseRngData);
|
||
// Estimate the terrain offset (runs a one state EKF)
|
||
EstimateTerrainOffset();
|
||
// Indicate we have used the range data
|
||
newDataRng = false;
|
||
}
|
||
|
||
// Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
|
||
if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE)
|
||
{
|
||
// Set the flow noise used by the fusion processes
|
||
R_LOS = sq(max(frontend._flowNoise, 0.05f));
|
||
// ensure that the covariance prediction is up to date before fusing data
|
||
if (!covPredStep) CovariancePrediction();
|
||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||
FuseOptFlow();
|
||
// reset flag to indicate that no new flow data is available for fusion
|
||
newDataFlow = false;
|
||
}
|
||
|
||
// stop the performance timer
|
||
perf_end(_perf_FuseOptFlow);
|
||
}
|
||
|
||
// update the quaternion, velocity and position states using delayed IMU measurements
|
||
// because the EKF is running on a delayed time horizon
|
||
void NavEKF2_core::UpdateStrapdownEquationsNED()
|
||
{
|
||
Vector3f delVelNav; // delta velocity vector
|
||
|
||
// remove gyro scale factor errors
|
||
correctedDelAng.x = imuDataDelayed.delAng.x * stateStruct.gyro_scale.x;
|
||
correctedDelAng.y = imuDataDelayed.delAng.y * stateStruct.gyro_scale.y;
|
||
correctedDelAng.z = imuDataDelayed.delAng.z * stateStruct.gyro_scale.z;
|
||
|
||
// remove sensor bias errors
|
||
correctedDelAng -= stateStruct.gyro_bias;
|
||
correctedDelVel = imuDataDelayed.delVel;
|
||
correctedDelVel.z -= stateStruct.accel_zbias;
|
||
|
||
// apply correction for earths rotation rate
|
||
// % * - and + operators have been overloaded
|
||
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT;
|
||
|
||
// convert the rotation vector to its equivalent quaternion
|
||
correctedDelAngQuat.from_axis_angle(correctedDelAng);
|
||
|
||
// update the quaternion states by rotating from the previous attitude through
|
||
// the delta angle rotation quaternion and normalise
|
||
stateStruct.quat *= correctedDelAngQuat;
|
||
stateStruct.quat.normalize();
|
||
|
||
// calculate the body to nav cosine matrix
|
||
Matrix3f Tbn_temp;
|
||
stateStruct.quat.rotation_matrix(Tbn_temp);
|
||
prevTnb = Tbn_temp.transposed();
|
||
|
||
// transform body delta velocities to delta velocities in the nav frame
|
||
// * and + operators have been overloaded
|
||
delVelNav = Tbn_temp*correctedDelVel;
|
||
delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
|
||
|
||
// calculate the rate of change of velocity (used for launch detect and other functions)
|
||
velDotNED = delVelNav / imuDataDelayed.delVelDT;
|
||
|
||
// apply a first order lowpass filter
|
||
velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
|
||
|
||
// calculate a magnitude of the filtered nav acceleration (required for GPS
|
||
// variance estimation)
|
||
accNavMag = velDotNEDfilt.length();
|
||
accNavMagHoriz = pythagorous2(velDotNEDfilt.x , velDotNEDfilt.y);
|
||
|
||
// save velocity for use in trapezoidal intergration for position calcuation
|
||
Vector3f lastVelocity = stateStruct.velocity;
|
||
|
||
// sum delta velocities to get velocity
|
||
stateStruct.velocity += delVelNav;
|
||
|
||
// apply a trapezoidal integration to velocities to calculate position
|
||
stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
|
||
|
||
// accumulate the bias delta angle and time since last reset by an OF measurement arrival
|
||
delAngBodyOF += imuDataNew.delAng - stateStruct.gyro_bias;
|
||
delTimeOF += imuDataNew.delAngDT;
|
||
|
||
// limit states to protect against divergence
|
||
ConstrainStates();
|
||
}
|
||
|
||
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
|
||
// using buffered IMU data
|
||
void NavEKF2_core::calcOutputStates() {
|
||
|
||
// initialise the store access at the fusion time horizon (it will be advanced later)
|
||
uint8_t imuStoreAccessIndex = fifoIndexDelayed;
|
||
imu_elements imuData;
|
||
|
||
// Counter used to ensure the while loop always exits
|
||
uint8_t watchdog = 0;
|
||
|
||
// initialise to the solution at the fusion time horizon
|
||
outputDataNew.quat = stateStruct.quat;
|
||
outputDataNew.velocity = stateStruct.velocity;
|
||
outputDataNew.position = stateStruct.position;
|
||
|
||
// Iterate through the buffered IMU data, using the strapdown equations to wind forward from the fusion time horizon to current time
|
||
// we stop iterating when we have reached the current imu Data
|
||
do {
|
||
|
||
// If the loop cannot exit, force exit
|
||
if (watchdog > IMU_BUFFER_LENGTH+2) {
|
||
return;
|
||
}
|
||
watchdog++;
|
||
|
||
// advance to the next index
|
||
imuStoreAccessIndex++;
|
||
|
||
// if we have got to the end of the array, return to the start
|
||
if (imuStoreAccessIndex >= IMU_BUFFER_LENGTH) {
|
||
imuStoreAccessIndex = 0;
|
||
}
|
||
imuData = storedIMU[imuStoreAccessIndex];
|
||
|
||
// remove gyro bias errors
|
||
Vector3f delAng = imuData.delAng - stateStruct.gyro_bias;
|
||
|
||
// remove Z accel bias error
|
||
Vector3f delVel = imuData.delVel;
|
||
delVel.z -= stateStruct.accel_zbias;
|
||
|
||
// convert the rotation vector to its equivalent quaternion
|
||
Quaternion deltaQuat;
|
||
deltaQuat.from_axis_angle_fast(delAng);
|
||
|
||
// update the quaternion states by rotating from the previous attitude through
|
||
// the delta angle rotation quaternion and normalise
|
||
outputDataNew.quat *= deltaQuat;
|
||
outputDataNew.quat.normalize();
|
||
|
||
// calculate the body to nav cosine matrix
|
||
Matrix3f Tbn_temp;
|
||
outputDataNew.quat.rotation_matrix(Tbn_temp);
|
||
|
||
// transform body delta velocities to delta velocities in the nav frame
|
||
// * and + operators have been overloaded
|
||
Vector3f delVelNav = Tbn_temp*delVel;
|
||
delVelNav.z += GRAVITY_MSS*imuData.delVelDT;
|
||
|
||
// use a simple Euler integration
|
||
outputDataNew.position += outputDataNew.velocity*imuData.delVelDT;
|
||
}
|
||
while (imuStoreAccessIndex != fifoIndexNow);
|
||
}
|
||
|
||
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
|
||
// using simple observer. This also applies an LPF to fusion corrections
|
||
|
||
void NavEKF2_core::calcOutputStatesFast() {
|
||
|
||
// Calculate strapdown solution at the current time horizon
|
||
|
||
// remove gyro scale factor errors
|
||
Vector3f delAng;
|
||
delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x;
|
||
delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y;
|
||
delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z;
|
||
|
||
// remove sensor bias errors
|
||
delAng -= stateStruct.gyro_bias;
|
||
Vector3f delVel;
|
||
delVel = imuDataNew.delVel;
|
||
delVel.z -= stateStruct.accel_zbias;
|
||
|
||
// apply corections to track EKF solution
|
||
delAng += delAngCorrection;
|
||
|
||
// convert the rotation vector to its equivalent quaternion
|
||
Quaternion deltaQuat;
|
||
deltaQuat.from_axis_angle(delAng);
|
||
|
||
// update the quaternion states by rotating from the previous attitude through
|
||
// the delta angle rotation quaternion and normalise
|
||
outputDataNew.quat *= deltaQuat;
|
||
outputDataNew.quat.normalize();
|
||
|
||
// calculate the body to nav cosine matrix
|
||
Matrix3f Tbn_temp;
|
||
outputDataNew.quat.rotation_matrix(Tbn_temp);
|
||
|
||
// transform body delta velocities to delta velocities in the nav frame
|
||
// Add the earth frame correction required to track the EKF states
|
||
// * and + operators have been overloaded
|
||
Vector3f delVelNav = Tbn_temp*delVel + delVelCorrection;
|
||
delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
|
||
|
||
// save velocity for use in trapezoidal intergration for position calcuation
|
||
Vector3f lastVelocity = outputDataNew.velocity;
|
||
|
||
// sum delta velocities to get velocity
|
||
outputDataNew.velocity += delVelNav;
|
||
|
||
// apply a trapezoidal integration to velocities to calculate position, applying correction required to track EKF solution
|
||
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f) + velCorrection * imuDataNew.delVelDT;
|
||
|
||
// store the output in the FIFO buffer
|
||
StoreOutput();
|
||
|
||
// extract data at the fusion time horizon from the FIFO buffer
|
||
RecallOutput();
|
||
|
||
// compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
|
||
|
||
// divide the demanded quaternion by the estimated to get the error
|
||
Quaternion quatErr = stateStruct.quat / outputDataDelayed.quat;
|
||
|
||
// Convert to a delta rotation using a small angle approximation
|
||
quatErr.normalize();
|
||
Vector3f deltaAngErr;
|
||
float scaler;
|
||
if (quatErr[0] >= 0.0f) {
|
||
scaler = 2.0f;
|
||
} else {
|
||
scaler = -2.0f;
|
||
}
|
||
deltaAngErr.x = scaler * quatErr[1];
|
||
deltaAngErr.y = scaler * quatErr[2];
|
||
deltaAngErr.z = scaler * quatErr[3];
|
||
|
||
// multiply the angle error vector by a gain to calculate the delta angle correction required to track the EKF solution
|
||
const float Kang = 1.0f;
|
||
delAngCorrection = deltaAngErr * imuDataNew.delAngDT * Kang;
|
||
|
||
// multiply velocity error by a gain to calculate the delta velocity correction required to track the EKF solution
|
||
const float Kvel = 1.0f;
|
||
delVelCorrection = (stateStruct.velocity - outputDataDelayed.velocity) * imuDataNew.delVelDT * Kvel;
|
||
|
||
// multiply position error by a gain to calculate the velocity correction required to track the EKF solution
|
||
const float Kpos = 1.0f;
|
||
velCorrection = (stateStruct.position - outputDataDelayed.position) * Kpos;
|
||
|
||
}
|
||
|
||
// calculate the predicted state covariance matrix
|
||
void NavEKF2_core::CovariancePrediction()
|
||
{
|
||
perf_begin(_perf_CovariancePrediction);
|
||
float windVelSigma; // wind velocity 1-sigma process noise - m/s
|
||
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
|
||
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
|
||
float dAngScaleSigma;// delta angle scale factor 1-Sigma process noise
|
||
float magEarthSigma;// earth magnetic field 1-sigma process noise
|
||
float magBodySigma; // body magnetic field 1-sigma process noise
|
||
float daxNoise; // X axis delta angle noise (rad)
|
||
float dayNoise; // Y axis delta angle noise (rad)
|
||
float dazNoise; // Z axis delta angle noise (rad)
|
||
float dvxNoise; // X axis delta velocity noise (m/s)
|
||
float dvyNoise; // Y axis delta velocity noise (m/s)
|
||
float dvzNoise; // Z axis delta velocity noise (m/s)
|
||
float dvx; // X axis delta velocity (m/s)
|
||
float dvy; // Y axis delta velocity (m/s)
|
||
float dvz; // Z axis delta velocity (m/s)
|
||
float dax; // X axis delta angle (rad)
|
||
float day; // Y axis delta angle (rad)
|
||
float daz; // Z axis delta angle (rad)
|
||
float q0; // attitude quaternion
|
||
float q1; // attitude quaternion
|
||
float q2; // attitude quaternion
|
||
float q3; // attitude quaternion
|
||
float dax_b; // X axis delta angle measurement bias (rad)
|
||
float day_b; // Y axis delta angle measurement bias (rad)
|
||
float daz_b; // Z axis delta angle measurement bias (rad)
|
||
float dax_s; // X axis delta angle measurement scale factor
|
||
float day_s; // Y axis delta angle measurement scale factor
|
||
float daz_s; // Z axis delta angle measurement scale factor
|
||
float dvz_b; // Z axis delta velocity measurement bias (rad)
|
||
|
||
// calculate covariance prediction process noise
|
||
// use filtered height rate to increase wind process noise when climbing or descending
|
||
// this allows for wind gradient effects.
|
||
// filter height rate using a 10 second time constant filter
|
||
float alpha = 0.1f * dt;
|
||
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
|
||
|
||
// use filtered height rate to increase wind process noise when climbing or descending
|
||
// this allows for wind gradient effects.
|
||
windVelSigma = dt * constrain_float(frontend._windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(frontend._wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
|
||
dAngBiasSigma = dt * constrain_float(frontend._gyroBiasProcessNoise, 1e-8f, 1e-4f);
|
||
dVelBiasSigma = dt * constrain_float(frontend._accelBiasProcessNoise, 1e-6f, 1e-2f);
|
||
dAngScaleSigma = dt * constrain_float(frontend._gyroScaleProcessNoise,1e-8f,1e-5f);
|
||
magEarthSigma = dt * constrain_float(frontend._magProcessNoise, 1e-4f, 1e-2f);
|
||
magBodySigma = dt * constrain_float(frontend._magProcessNoise, 1e-4f, 1e-2f);
|
||
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 1.0e-9f;
|
||
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
|
||
for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
|
||
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
|
||
if (expectGndEffectTakeoff) {
|
||
processNoise[15] = 0.0f;
|
||
} else if (!filterArmed) {
|
||
processNoise[15] = dVelBiasSigma * frontend.accelBiasNoiseScaler;
|
||
} else {
|
||
processNoise[15] = dVelBiasSigma;
|
||
}
|
||
for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;
|
||
for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma;
|
||
for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma;
|
||
|
||
for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);
|
||
|
||
// set variables used to calculate covariance growth
|
||
dvx = summedDelVel.x;
|
||
dvy = summedDelVel.y;
|
||
dvz = summedDelVel.z;
|
||
dax = summedDelAng.x;
|
||
day = summedDelAng.y;
|
||
daz = summedDelAng.z;
|
||
q0 = stateStruct.quat[0];
|
||
q1 = stateStruct.quat[1];
|
||
q2 = stateStruct.quat[2];
|
||
q3 = stateStruct.quat[3];
|
||
dax_b = stateStruct.gyro_bias.x;
|
||
day_b = stateStruct.gyro_bias.y;
|
||
daz_b = stateStruct.gyro_bias.z;
|
||
dax_s = stateStruct.gyro_scale.x;
|
||
day_s = stateStruct.gyro_scale.y;
|
||
daz_s = stateStruct.gyro_scale.z;
|
||
dvz_b = stateStruct.accel_zbias;
|
||
float _gyrNoise = constrain_float(frontend._gyrNoise, 1e-3f, 5e-2f);
|
||
daxNoise = dt*_gyrNoise;
|
||
dayNoise = dt*_gyrNoise;
|
||
// Account for 3% scale factor error on Z angular rate. This reduces chance of continuous fast rotations causing loss of yaw reference.
|
||
dazNoise = dt*(pythagorous2(_gyrNoise,0.03f*yawRateFilt));
|
||
float _accNoise = constrain_float(frontend._accNoise, 5e-2f, 1.0f);
|
||
dvxNoise = dt*_accNoise;
|
||
dvyNoise = dt*_accNoise;
|
||
dvzNoise = dt*_accNoise;
|
||
|
||
// calculate the predicted covariance due to inertial sensor error propagation
|
||
// we calculate the upper diagonal and copy to take advantage of symmetry
|
||
SF[0] = daz_b/2 + dazNoise/2 - (daz*daz_s)/2;
|
||
SF[1] = day_b/2 + dayNoise/2 - (day*day_s)/2;
|
||
SF[2] = dax_b/2 + daxNoise/2 - (dax*dax_s)/2;
|
||
SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2;
|
||
SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2;
|
||
SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2;
|
||
SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2;
|
||
SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2;
|
||
SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2;
|
||
SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2;
|
||
SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2;
|
||
SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2;
|
||
SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2;
|
||
SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2;
|
||
SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2;
|
||
SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
|
||
SF[16] = dvz_b - dvz + dvzNoise;
|
||
SF[17] = dvx - dvxNoise;
|
||
SF[18] = dvy - dvyNoise;
|
||
SF[19] = sq(q2);
|
||
SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3);
|
||
SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3);
|
||
SF[22] = 2*q0*q1 - 2*q2*q3;
|
||
SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3);
|
||
SF[24] = 2*q1*q2;
|
||
|
||
SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
|
||
SG[1] = sq(q3);
|
||
SG[2] = sq(q2);
|
||
SG[3] = sq(q1);
|
||
SG[4] = sq(q0);
|
||
|
||
SQ[0] = - dvyNoise*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
|
||
SQ[1] = dvxNoise*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
|
||
SQ[2] = dvyNoise*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
|
||
SQ[3] = sq(SG[0]);
|
||
SQ[4] = 2*q2*q3;
|
||
SQ[5] = 2*q1*q3;
|
||
SQ[6] = 2*q1*q2;
|
||
SQ[7] = SG[4];
|
||
|
||
SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
|
||
SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
|
||
SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];
|
||
SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10];
|
||
SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12];
|
||
SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14];
|
||
SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12];
|
||
SPP[7] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3);
|
||
SPP[8] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9];
|
||
SPP[9] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9];
|
||
SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3);
|
||
SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3);
|
||
SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3);
|
||
SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9];
|
||
SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13];
|
||
SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3);
|
||
SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3);
|
||
SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3);
|
||
SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3);
|
||
SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3);
|
||
SPP[20] = SF[16]*SF[21] - SF[18]*SF[22];
|
||
SPP[21] = 2*q0*q2 + 2*q1*q3;
|
||
SPP[22] = SF[15];
|
||
|
||
if (inhibitMagStates) {
|
||
zeroRows(P,16,21);
|
||
zeroCols(P,16,21);
|
||
} else if (inhibitWindStates) {
|
||
zeroRows(P,22,23);
|
||
zeroCols(P,22,23);
|
||
}
|
||
|
||
nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[9][0]*SPP[22] + P[12][0]*SPP[18] + P[2][0]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[9][1]*SPP[22] + P[12][1]*SPP[18] + P[2][1]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[8]*(P[0][2]*SPP[5] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18] - P[1][2]*(2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12])) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[9][9]*SPP[22] + P[12][9]*SPP[18] + P[2][9]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[9][12]*SPP[22] + P[12][12]*SPP[18] + P[2][12]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]));
|
||
nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18]) - (2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9])*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
|
||
nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[9]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);
|
||
nextP[0][2] = SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]);
|
||
nextP[1][2] = SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]);
|
||
nextP[2][2] = dazNoise*SQ[3] - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]);
|
||
nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
|
||
nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
|
||
nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
|
||
nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + dvyNoise*sq(SQ[6] - 2*q0*q3) + dvzNoise*sq(SQ[5] + 2*q0*q2) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[2][15]*SPP[15] - P[15][15]*SPP[21] + P[1][15]*(SF[16]*SF[23] - SF[17]*SPP[21])) + dvxNoise*sq(SG[1] + SG[2] - SG[3] - SQ[7]);
|
||
nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
|
||
nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]);
|
||
nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]);
|
||
nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]);
|
||
nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + dvxNoise*sq(SQ[6] + 2*q0*q3) + dvzNoise*sq(SQ[4] - 2*q0*q1) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + dvyNoise*sq(SG[1] - SG[2] + SG[3] - SQ[7]);
|
||
nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[7]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
|
||
nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[7]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
|
||
nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[7]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
|
||
nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[7]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]);
|
||
nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[7]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]);
|
||
nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + dvxNoise*sq(SQ[5] - 2*q0*q2) + dvyNoise*sq(SQ[4] + 2*q0*q1) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[7]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[7] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[7] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[7] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + dvzNoise*sq(SG[1] - SG[2] - SG[3] + SQ[7]);
|
||
nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[8] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18]);
|
||
nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[9] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17]);
|
||
nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]);
|
||
nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]);
|
||
nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]);
|
||
nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[7] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[7] + P[1][3]*SPP[10] + P[2][3]*SPP[0]);
|
||
nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt);
|
||
nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[8] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18]);
|
||
nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[9] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17]);
|
||
nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]);
|
||
nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]);
|
||
nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]);
|
||
nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[7] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[7] + P[1][4]*SPP[10] + P[2][4]*SPP[0]);
|
||
nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt);
|
||
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
|
||
nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[8] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18]);
|
||
nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[9] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17]);
|
||
nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]);
|
||
nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]);
|
||
nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]);
|
||
nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[7] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0]);
|
||
nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt);
|
||
nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
|
||
nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
|
||
nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[8] + P[9][9]*SPP[22] + P[12][9]*SPP[18];
|
||
nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[9] + P[10][9]*SPP[22] + P[13][9]*SPP[17];
|
||
nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16];
|
||
nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21];
|
||
nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11];
|
||
nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[7] + P[1][9]*SPP[10] + P[2][9]*SPP[0];
|
||
nextP[6][9] = P[6][9] + P[3][9]*dt;
|
||
nextP[7][9] = P[7][9] + P[4][9]*dt;
|
||
nextP[8][9] = P[8][9] + P[5][9]*dt;
|
||
nextP[9][9] = P[9][9];
|
||
nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18];
|
||
nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17];
|
||
nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16];
|
||
nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21];
|
||
nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11];
|
||
nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[7] + P[1][10]*SPP[10] + P[2][10]*SPP[0];
|
||
nextP[6][10] = P[6][10] + P[3][10]*dt;
|
||
nextP[7][10] = P[7][10] + P[4][10]*dt;
|
||
nextP[8][10] = P[8][10] + P[5][10]*dt;
|
||
nextP[9][10] = P[9][10];
|
||
nextP[10][10] = P[10][10];
|
||
nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18];
|
||
nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17];
|
||
nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16];
|
||
nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21];
|
||
nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11];
|
||
nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[7] + P[1][11]*SPP[10] + P[2][11]*SPP[0];
|
||
nextP[6][11] = P[6][11] + P[3][11]*dt;
|
||
nextP[7][11] = P[7][11] + P[4][11]*dt;
|
||
nextP[8][11] = P[8][11] + P[5][11]*dt;
|
||
nextP[9][11] = P[9][11];
|
||
nextP[10][11] = P[10][11];
|
||
nextP[11][11] = P[11][11];
|
||
nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[8] + P[9][12]*SPP[22] + P[12][12]*SPP[18];
|
||
nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[9] + P[10][12]*SPP[22] + P[13][12]*SPP[17];
|
||
nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16];
|
||
nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21];
|
||
nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11];
|
||
nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[7] + P[1][12]*SPP[10] + P[2][12]*SPP[0];
|
||
nextP[6][12] = P[6][12] + P[3][12]*dt;
|
||
nextP[7][12] = P[7][12] + P[4][12]*dt;
|
||
nextP[8][12] = P[8][12] + P[5][12]*dt;
|
||
nextP[9][12] = P[9][12];
|
||
nextP[10][12] = P[10][12];
|
||
nextP[11][12] = P[11][12];
|
||
nextP[12][12] = P[12][12];
|
||
nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18];
|
||
nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17];
|
||
nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16];
|
||
nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21];
|
||
nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11];
|
||
nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[7] + P[1][13]*SPP[10] + P[2][13]*SPP[0];
|
||
nextP[6][13] = P[6][13] + P[3][13]*dt;
|
||
nextP[7][13] = P[7][13] + P[4][13]*dt;
|
||
nextP[8][13] = P[8][13] + P[5][13]*dt;
|
||
nextP[9][13] = P[9][13];
|
||
nextP[10][13] = P[10][13];
|
||
nextP[11][13] = P[11][13];
|
||
nextP[12][13] = P[12][13];
|
||
nextP[13][13] = P[13][13];
|
||
nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18];
|
||
nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17];
|
||
nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16];
|
||
nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21];
|
||
nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11];
|
||
nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[7] + P[1][14]*SPP[10] + P[2][14]*SPP[0];
|
||
nextP[6][14] = P[6][14] + P[3][14]*dt;
|
||
nextP[7][14] = P[7][14] + P[4][14]*dt;
|
||
nextP[8][14] = P[8][14] + P[5][14]*dt;
|
||
nextP[9][14] = P[9][14];
|
||
nextP[10][14] = P[10][14];
|
||
nextP[11][14] = P[11][14];
|
||
nextP[12][14] = P[12][14];
|
||
nextP[13][14] = P[13][14];
|
||
nextP[14][14] = P[14][14];
|
||
nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18];
|
||
nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17];
|
||
nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16];
|
||
nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21];
|
||
nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11];
|
||
nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0];
|
||
nextP[6][15] = P[6][15] + P[3][15]*dt;
|
||
nextP[7][15] = P[7][15] + P[4][15]*dt;
|
||
nextP[8][15] = P[8][15] + P[5][15]*dt;
|
||
nextP[9][15] = P[9][15];
|
||
nextP[10][15] = P[10][15];
|
||
nextP[11][15] = P[11][15];
|
||
nextP[12][15] = P[12][15];
|
||
nextP[13][15] = P[13][15];
|
||
nextP[14][15] = P[14][15];
|
||
nextP[15][15] = P[15][15];
|
||
|
||
if (stateIndexLim > 15) {
|
||
nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[8] + P[9][16]*SPP[22] + P[12][16]*SPP[18];
|
||
nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[9] + P[10][16]*SPP[22] + P[13][16]*SPP[17];
|
||
nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16];
|
||
nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21];
|
||
nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11];
|
||
nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[7] + P[1][16]*SPP[10] + P[2][16]*SPP[0];
|
||
nextP[6][16] = P[6][16] + P[3][16]*dt;
|
||
nextP[7][16] = P[7][16] + P[4][16]*dt;
|
||
nextP[8][16] = P[8][16] + P[5][16]*dt;
|
||
nextP[9][16] = P[9][16];
|
||
nextP[10][16] = P[10][16];
|
||
nextP[11][16] = P[11][16];
|
||
nextP[12][16] = P[12][16];
|
||
nextP[13][16] = P[13][16];
|
||
nextP[14][16] = P[14][16];
|
||
nextP[15][16] = P[15][16];
|
||
nextP[16][16] = P[16][16];
|
||
nextP[0][17] = P[0][17]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[8] + P[9][17]*SPP[22] + P[12][17]*SPP[18];
|
||
nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[9] + P[10][17]*SPP[22] + P[13][17]*SPP[17];
|
||
nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16];
|
||
nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21];
|
||
nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11];
|
||
nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[7] + P[1][17]*SPP[10] + P[2][17]*SPP[0];
|
||
nextP[6][17] = P[6][17] + P[3][17]*dt;
|
||
nextP[7][17] = P[7][17] + P[4][17]*dt;
|
||
nextP[8][17] = P[8][17] + P[5][17]*dt;
|
||
nextP[9][17] = P[9][17];
|
||
nextP[10][17] = P[10][17];
|
||
nextP[11][17] = P[11][17];
|
||
nextP[12][17] = P[12][17];
|
||
nextP[13][17] = P[13][17];
|
||
nextP[14][17] = P[14][17];
|
||
nextP[15][17] = P[15][17];
|
||
nextP[16][17] = P[16][17];
|
||
nextP[17][17] = P[17][17];
|
||
nextP[0][18] = P[0][18]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[8] + P[9][18]*SPP[22] + P[12][18]*SPP[18];
|
||
nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[9] + P[10][18]*SPP[22] + P[13][18]*SPP[17];
|
||
nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16];
|
||
nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21];
|
||
nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11];
|
||
nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[7] + P[1][18]*SPP[10] + P[2][18]*SPP[0];
|
||
nextP[6][18] = P[6][18] + P[3][18]*dt;
|
||
nextP[7][18] = P[7][18] + P[4][18]*dt;
|
||
nextP[8][18] = P[8][18] + P[5][18]*dt;
|
||
nextP[9][18] = P[9][18];
|
||
nextP[10][18] = P[10][18];
|
||
nextP[11][18] = P[11][18];
|
||
nextP[12][18] = P[12][18];
|
||
nextP[13][18] = P[13][18];
|
||
nextP[14][18] = P[14][18];
|
||
nextP[15][18] = P[15][18];
|
||
nextP[16][18] = P[16][18];
|
||
nextP[17][18] = P[17][18];
|
||
nextP[18][18] = P[18][18];
|
||
nextP[0][19] = P[0][19]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[8] + P[9][19]*SPP[22] + P[12][19]*SPP[18];
|
||
nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[9] + P[10][19]*SPP[22] + P[13][19]*SPP[17];
|
||
nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16];
|
||
nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21];
|
||
nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11];
|
||
nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[7] + P[1][19]*SPP[10] + P[2][19]*SPP[0];
|
||
nextP[6][19] = P[6][19] + P[3][19]*dt;
|
||
nextP[7][19] = P[7][19] + P[4][19]*dt;
|
||
nextP[8][19] = P[8][19] + P[5][19]*dt;
|
||
nextP[9][19] = P[9][19];
|
||
nextP[10][19] = P[10][19];
|
||
nextP[11][19] = P[11][19];
|
||
nextP[12][19] = P[12][19];
|
||
nextP[13][19] = P[13][19];
|
||
nextP[14][19] = P[14][19];
|
||
nextP[15][19] = P[15][19];
|
||
nextP[16][19] = P[16][19];
|
||
nextP[17][19] = P[17][19];
|
||
nextP[18][19] = P[18][19];
|
||
nextP[19][19] = P[19][19];
|
||
nextP[0][20] = P[0][20]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[8] + P[9][20]*SPP[22] + P[12][20]*SPP[18];
|
||
nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[9] + P[10][20]*SPP[22] + P[13][20]*SPP[17];
|
||
nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16];
|
||
nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21];
|
||
nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11];
|
||
nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[7] + P[1][20]*SPP[10] + P[2][20]*SPP[0];
|
||
nextP[6][20] = P[6][20] + P[3][20]*dt;
|
||
nextP[7][20] = P[7][20] + P[4][20]*dt;
|
||
nextP[8][20] = P[8][20] + P[5][20]*dt;
|
||
nextP[9][20] = P[9][20];
|
||
nextP[10][20] = P[10][20];
|
||
nextP[11][20] = P[11][20];
|
||
nextP[12][20] = P[12][20];
|
||
nextP[13][20] = P[13][20];
|
||
nextP[14][20] = P[14][20];
|
||
nextP[15][20] = P[15][20];
|
||
nextP[16][20] = P[16][20];
|
||
nextP[17][20] = P[17][20];
|
||
nextP[18][20] = P[18][20];
|
||
nextP[19][20] = P[19][20];
|
||
nextP[20][20] = P[20][20];
|
||
nextP[0][21] = P[0][21]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[8] + P[9][21]*SPP[22] + P[12][21]*SPP[18];
|
||
nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[9] + P[10][21]*SPP[22] + P[13][21]*SPP[17];
|
||
nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16];
|
||
nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21];
|
||
nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11];
|
||
nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[7] + P[1][21]*SPP[10] + P[2][21]*SPP[0];
|
||
nextP[6][21] = P[6][21] + P[3][21]*dt;
|
||
nextP[7][21] = P[7][21] + P[4][21]*dt;
|
||
nextP[8][21] = P[8][21] + P[5][21]*dt;
|
||
nextP[9][21] = P[9][21];
|
||
nextP[10][21] = P[10][21];
|
||
nextP[11][21] = P[11][21];
|
||
nextP[12][21] = P[12][21];
|
||
nextP[13][21] = P[13][21];
|
||
nextP[14][21] = P[14][21];
|
||
nextP[15][21] = P[15][21];
|
||
nextP[16][21] = P[16][21];
|
||
nextP[17][21] = P[17][21];
|
||
nextP[18][21] = P[18][21];
|
||
nextP[19][21] = P[19][21];
|
||
nextP[20][21] = P[20][21];
|
||
nextP[21][21] = P[21][21];
|
||
|
||
if (stateIndexLim > 21) {
|
||
nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[8] + P[9][22]*SPP[22] + P[12][22]*SPP[18];
|
||
nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[9] + P[10][22]*SPP[22] + P[13][22]*SPP[17];
|
||
nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16];
|
||
nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21];
|
||
nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11];
|
||
nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[7] + P[1][22]*SPP[10] + P[2][22]*SPP[0];
|
||
nextP[6][22] = P[6][22] + P[3][22]*dt;
|
||
nextP[7][22] = P[7][22] + P[4][22]*dt;
|
||
nextP[8][22] = P[8][22] + P[5][22]*dt;
|
||
nextP[9][22] = P[9][22];
|
||
nextP[10][22] = P[10][22];
|
||
nextP[11][22] = P[11][22];
|
||
nextP[12][22] = P[12][22];
|
||
nextP[13][22] = P[13][22];
|
||
nextP[14][22] = P[14][22];
|
||
nextP[15][22] = P[15][22];
|
||
nextP[16][22] = P[16][22];
|
||
nextP[17][22] = P[17][22];
|
||
nextP[18][22] = P[18][22];
|
||
nextP[19][22] = P[19][22];
|
||
nextP[20][22] = P[20][22];
|
||
nextP[21][22] = P[21][22];
|
||
nextP[22][22] = P[22][22];
|
||
nextP[0][23] = P[0][23]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[8] + P[9][23]*SPP[22] + P[12][23]*SPP[18];
|
||
nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[9] + P[10][23]*SPP[22] + P[13][23]*SPP[17];
|
||
nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16];
|
||
nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21];
|
||
nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11];
|
||
nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[7] + P[1][23]*SPP[10] + P[2][23]*SPP[0];
|
||
nextP[6][23] = P[6][23] + P[3][23]*dt;
|
||
nextP[7][23] = P[7][23] + P[4][23]*dt;
|
||
nextP[8][23] = P[8][23] + P[5][23]*dt;
|
||
nextP[9][23] = P[9][23];
|
||
nextP[10][23] = P[10][23];
|
||
nextP[11][23] = P[11][23];
|
||
nextP[12][23] = P[12][23];
|
||
nextP[13][23] = P[13][23];
|
||
nextP[14][23] = P[14][23];
|
||
nextP[15][23] = P[15][23];
|
||
nextP[16][23] = P[16][23];
|
||
nextP[17][23] = P[17][23];
|
||
nextP[18][23] = P[18][23];
|
||
nextP[19][23] = P[19][23];
|
||
nextP[20][23] = P[20][23];
|
||
nextP[21][23] = P[21][23];
|
||
nextP[22][23] = P[22][23];
|
||
nextP[23][23] = P[23][23];
|
||
}
|
||
}
|
||
|
||
// Copy upper diagonal to lower diagonal taking advantage of symmetry
|
||
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++)
|
||
{
|
||
for (uint8_t rowIndex=0; rowIndex<colIndex; rowIndex++)
|
||
{
|
||
nextP[colIndex][rowIndex] = nextP[rowIndex][colIndex];
|
||
}
|
||
}
|
||
|
||
// add the general state process noise variances
|
||
for (uint8_t i=0; i<=stateIndexLim; i++)
|
||
{
|
||
nextP[i][i] = nextP[i][i] + processNoise[i];
|
||
}
|
||
|
||
// if the total position variance exceeds 1e4 (100m), then stop covariance
|
||
// growth by setting the predicted to the previous values
|
||
// This prevent an ill conditioned matrix from occurring for long periods
|
||
// without GPS
|
||
if ((P[6][6] + P[7][7]) > 1e4f)
|
||
{
|
||
for (uint8_t i=6; i<=7; i++)
|
||
{
|
||
for (uint8_t j=0; j<=stateIndexLim; j++)
|
||
{
|
||
nextP[i][j] = P[i][j];
|
||
nextP[j][i] = P[j][i];
|
||
}
|
||
}
|
||
}
|
||
|
||
// copy covariances to output
|
||
CopyCovariances();
|
||
|
||
// constrain diagonals to prevent ill-conditioning
|
||
ConstrainVariances();
|
||
|
||
// set the flag to indicate that covariance prediction has been performed and reset the increments used by the covariance prediction
|
||
covPredStep = true;
|
||
summedDelAng.zero();
|
||
summedDelVel.zero();
|
||
dt = 0.0f;
|
||
|
||
perf_end(_perf_CovariancePrediction);
|
||
}
|
||
|
||
// fuse selected position, velocity and height measurements
|
||
void NavEKF2_core::FuseVelPosNED()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_FuseVelPosNED);
|
||
|
||
// health is set bad until test passed
|
||
velHealth = false;
|
||
posHealth = false;
|
||
hgtHealth = false;
|
||
|
||
// declare variables used to check measurement errors
|
||
Vector3f velInnov;
|
||
|
||
// declare variables used to control access to arrays
|
||
bool fuseData[6] = {false,false,false,false,false,false};
|
||
uint8_t stateIndex;
|
||
uint8_t obsIndex;
|
||
|
||
// declare variables used by state and covariance update calculations
|
||
float posErr;
|
||
Vector6 R_OBS; // Measurement variances used for fusion
|
||
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
|
||
Vector6 observation;
|
||
float SK;
|
||
|
||
// perform sequential fusion of GPS measurements. This assumes that the
|
||
// errors in the different velocity and position components are
|
||
// uncorrelated which is not true, however in the absence of covariance
|
||
// data from the GPS receiver it is the only assumption we can make
|
||
// so we might as well take advantage of the computational efficiencies
|
||
// associated with sequential fusion
|
||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||
|
||
// set the GPS data timeout depending on whether airspeed data is present
|
||
uint32_t gpsRetryTime;
|
||
if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
|
||
else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
|
||
|
||
// form the observation vector and zero velocity and horizontal position observations if in constant position mode
|
||
// If in constant velocity mode, hold the last known horizontal velocity vector
|
||
if (!constPosMode && !constVelMode) {
|
||
observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
|
||
observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
|
||
observation[2] = gpsDataDelayed.vel.z;
|
||
observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
|
||
observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
|
||
} else if (constPosMode) {
|
||
for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
|
||
} else if (constVelMode) {
|
||
observation[0] = heldVelNE.x;
|
||
observation[1] = heldVelNE.y;
|
||
for (uint8_t i=2; i<=4; i++) observation[i] = 0.0f;
|
||
}
|
||
observation[5] = -baroDataDelayed.hgt;
|
||
|
||
// calculate additional error in GPS position caused by manoeuvring
|
||
posErr = frontend.gpsPosVarAccScale * accNavMag;
|
||
|
||
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
|
||
// otherwise we scale it using manoeuvre acceleration
|
||
if (gpsSpdAccuracy > 0.0f) {
|
||
// use GPS receivers reported speed accuracy - floor at value set by gps noise parameter
|
||
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
|
||
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
|
||
} else {
|
||
// calculate additional error in GPS velocity caused by manoeuvring
|
||
R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
|
||
R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale * accNavMag);
|
||
}
|
||
R_OBS[1] = R_OBS[0];
|
||
R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||
R_OBS[4] = R_OBS[3];
|
||
R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
|
||
|
||
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
||
if ((getTakeoffExpected() || getTouchdownExpected()) && filterArmed) {
|
||
R_OBS[5] *= frontend.gndEffectBaroScaler;
|
||
}
|
||
|
||
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
||
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
|
||
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
||
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
|
||
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
||
|
||
|
||
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
|
||
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
||
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
||
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend.hgtAvg_ms)) {
|
||
// calculate innovations for height and vertical GPS vel measurements
|
||
float hgtErr = stateStruct.position.z - observation[5];
|
||
float velDErr = stateStruct.velocity.z - observation[2];
|
||
// check if they are the same sign and both more than 3-sigma out of bounds
|
||
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
|
||
badIMUdata = true;
|
||
} else {
|
||
badIMUdata = false;
|
||
}
|
||
}
|
||
|
||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||
// test position measurements
|
||
if (fusePosData) {
|
||
// test horizontal position measurements
|
||
innovVelPos[3] = stateStruct.position.x - observation[3];
|
||
innovVelPos[4] = stateStruct.position.y - observation[4];
|
||
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
|
||
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
|
||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||
float maxPosInnov2 = sq(frontend._gpsPosInnovGate)*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
||
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
||
// declare a timeout condition if we have been too long without data or not aiding
|
||
posTimeout = (((imuSampleTime_ms - lastPosPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
|
||
// use position data if healthy, timed out, or in constant position mode
|
||
if (posHealth || posTimeout || constPosMode) {
|
||
posHealth = true;
|
||
// only reset the failed time and do glitch timeout checks if we are doing full aiding
|
||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||
lastPosPassTime_ms = imuSampleTime_ms;
|
||
// if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
|
||
if (posTimeout || ((varInnovVelPos[3] + varInnovVelPos[4]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
|
||
gpsPosGlitchOffsetNE.x += innovVelPos[3];
|
||
gpsPosGlitchOffsetNE.y += innovVelPos[4];
|
||
// limit the radius of the offset and decay the offset to zero radially
|
||
decayGpsOffset();
|
||
// reset the position to the current GPS position which will include the glitch correction offset
|
||
ResetPosition();
|
||
// reset the velocity to the GPS velocity
|
||
ResetVelocity();
|
||
// don't fuse data on this time step
|
||
fusePosData = false;
|
||
// record the fail time
|
||
lastPosFailTime_ms = imuSampleTime_ms;
|
||
// Reset the normalised innovation to avoid false failing the bad position fusion test
|
||
posTestRatio = 0.0f;
|
||
}
|
||
}
|
||
} else {
|
||
posHealth = false;
|
||
}
|
||
}
|
||
|
||
// test velocity measurements
|
||
if (fuseVelData) {
|
||
// test velocity measurements
|
||
uint8_t imax = 2;
|
||
if (frontend._fusionModeGPS == 1 || constVelMode) {
|
||
imax = 1;
|
||
}
|
||
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
||
float varVelSum = 0; // sum of velocity innovation variances
|
||
for (uint8_t i = 0; i<=imax; i++) {
|
||
// velocity states start at index 3
|
||
stateIndex = i + 3;
|
||
// calculate innovations using blended and single IMU predicted states
|
||
velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended
|
||
// calculate innovation variance
|
||
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
|
||
// sum the innovation and innovation variances
|
||
innovVelSumSq += sq(velInnov[i]);
|
||
varVelSum += varInnovVelPos[i];
|
||
}
|
||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||
// calculate the test ratio
|
||
velTestRatio = innovVelSumSq / (varVelSum * sq(frontend._gpsVelInnovGate));
|
||
// fail if the ratio is greater than 1
|
||
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
||
// declare a timeout if we have not fused velocity data for too long or not aiding
|
||
velTimeout = (((imuSampleTime_ms - lastVelPassTime_ms) > gpsRetryTime) || PV_AidingMode == AID_NONE);
|
||
// if data is healthy or in constant velocity mode we fuse it
|
||
if (velHealth || velTimeout || constVelMode) {
|
||
velHealth = true;
|
||
// restart the timeout count
|
||
lastVelPassTime_ms = imuSampleTime_ms;
|
||
} else if (velTimeout && !posHealth && PV_AidingMode == AID_ABSOLUTE) {
|
||
// if data is not healthy and timed out and position is unhealthy and we are using aiding, we reset the velocity, but do not fuse data on this time step
|
||
ResetVelocity();
|
||
fuseVelData = false;
|
||
} else {
|
||
// if data is unhealthy and position is healthy, we do not fuse it
|
||
velHealth = false;
|
||
}
|
||
}
|
||
|
||
// test height measurements
|
||
if (fuseHgtData) {
|
||
// calculate height innovations
|
||
innovVelPos[5] = stateStruct.position.z - observation[5];
|
||
|
||
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
|
||
// calculate the innovation consistency test ratio
|
||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend._hgtInnovGate) * varInnovVelPos[5]);
|
||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
||
hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms;
|
||
// Fuse height data if healthy or timed out or in constant position mode
|
||
if (hgtHealth || hgtTimeout || constPosMode) {
|
||
hgtHealth = true;
|
||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||
// if timed out, reset the height, but do not fuse data on this time step
|
||
if (hgtTimeout) {
|
||
ResetHeight();
|
||
fuseHgtData = false;
|
||
}
|
||
}
|
||
else {
|
||
hgtHealth = false;
|
||
}
|
||
}
|
||
|
||
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
|
||
if (fuseVelData && velHealth) {
|
||
fuseData[0] = true;
|
||
fuseData[1] = true;
|
||
if (useGpsVertVel) {
|
||
fuseData[2] = true;
|
||
}
|
||
tiltErrVec.zero();
|
||
}
|
||
if (fusePosData && posHealth) {
|
||
fuseData[3] = true;
|
||
fuseData[4] = true;
|
||
tiltErrVec.zero();
|
||
}
|
||
if (fuseHgtData && hgtHealth) {
|
||
fuseData[5] = true;
|
||
}
|
||
if (constVelMode) {
|
||
fuseData[0] = true;
|
||
fuseData[1] = true;
|
||
tiltErrVec.zero();
|
||
}
|
||
|
||
// fuse measurements sequentially
|
||
for (obsIndex=0; obsIndex<=5; obsIndex++) {
|
||
if (fuseData[obsIndex]) {
|
||
stateIndex = 3 + obsIndex;
|
||
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
|
||
// adjust scaling on GPS measurement noise variances if not enough satellites
|
||
if (obsIndex <= 2)
|
||
{
|
||
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
|
||
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
||
}
|
||
else if (obsIndex == 3 || obsIndex == 4) {
|
||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
||
} else {
|
||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||
if (obsIndex == 5) {
|
||
const float gndMaxBaroErr = 4.0f;
|
||
const float gndBaroInnovFloor = -0.5f;
|
||
|
||
if(getTouchdownExpected()) {
|
||
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
||
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
||
// this function looks like this:
|
||
// |/
|
||
//---------|---------
|
||
// ____/|
|
||
// / |
|
||
// / |
|
||
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
||
}
|
||
}
|
||
}
|
||
|
||
// calculate the Kalman gain and calculate innovation variances
|
||
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
|
||
SK = 1.0f/varInnovVelPos[obsIndex];
|
||
for (uint8_t i= 0; i<=15; i++) {
|
||
Kfusion[i] = P[i][stateIndex]*SK;
|
||
}
|
||
|
||
// inhibit magnetic field state estimation by setting Kalman gains to zero
|
||
if (!inhibitMagStates) {
|
||
for (uint8_t i = 16; i<=21; i++) {
|
||
Kfusion[i] = P[i][stateIndex]*SK;
|
||
}
|
||
} else {
|
||
for (uint8_t i = 16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// inhibit wind state estimation by setting Kalman gains to zero
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = P[22][stateIndex]*SK;
|
||
Kfusion[23] = P[23][stateIndex]*SK;
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
|
||
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
|
||
stateStruct.angErr.zero();
|
||
|
||
// calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
|
||
// Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// sum the attitude error from velocity and position fusion only
|
||
// used as a metric for convergence monitoring
|
||
if (obsIndex != 5) {
|
||
tiltErrVec += stateStruct.angErr;
|
||
}
|
||
|
||
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
|
||
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
|
||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++)
|
||
{
|
||
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
|
||
}
|
||
}
|
||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||
P[i][j] = P[i][j] - KHP[i][j];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
|
||
// stop performance timer
|
||
perf_end(_perf_FuseVelPosNED);
|
||
}
|
||
|
||
// fuse magnetometer measurements and apply innovation consistency checks
|
||
// fuse each axis on consecutive time steps to spread computional load
|
||
void NavEKF2_core::FuseMagnetometer()
|
||
{
|
||
// declarations
|
||
ftype &q0 = mag_state.q0;
|
||
ftype &q1 = mag_state.q1;
|
||
ftype &q2 = mag_state.q2;
|
||
ftype &q3 = mag_state.q3;
|
||
ftype &magN = mag_state.magN;
|
||
ftype &magE = mag_state.magE;
|
||
ftype &magD = mag_state.magD;
|
||
ftype &magXbias = mag_state.magXbias;
|
||
ftype &magYbias = mag_state.magYbias;
|
||
ftype &magZbias = mag_state.magZbias;
|
||
uint8_t &obsIndex = mag_state.obsIndex;
|
||
Matrix3f &DCM = mag_state.DCM;
|
||
Vector3f &MagPred = mag_state.MagPred;
|
||
ftype &R_MAG = mag_state.R_MAG;
|
||
ftype *SH_MAG = &mag_state.SH_MAG[0];
|
||
Vector24 H_MAG;
|
||
Vector6 SK_MX;
|
||
Vector6 SK_MY;
|
||
Vector6 SK_MZ;
|
||
|
||
// perform sequential fusion of magnetometer measurements.
|
||
// this assumes that the errors in the different components are
|
||
// uncorrelated which is not true, however in the absence of covariance
|
||
// data fit is the only assumption we can make
|
||
// so we might as well take advantage of the computational efficiencies
|
||
// associated with sequential fusion
|
||
// calculate observation jacobians and Kalman gains
|
||
if (obsIndex == 0)
|
||
{
|
||
// copy required states to local variable names
|
||
q0 = stateStruct.quat[0];
|
||
q1 = stateStruct.quat[1];
|
||
q2 = stateStruct.quat[2];
|
||
q3 = stateStruct.quat[3];
|
||
magN = stateStruct.earth_magfield[0];
|
||
magE = stateStruct.earth_magfield[1];
|
||
magD = stateStruct.earth_magfield[2];
|
||
magXbias = stateStruct.body_magfield[0];
|
||
magYbias = stateStruct.body_magfield[1];
|
||
magZbias = stateStruct.body_magfield[2];
|
||
|
||
// rotate predicted earth components into body axes and calculate
|
||
// predicted measurements
|
||
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
|
||
DCM[0][1] = 2*(q1*q2 + q0*q3);
|
||
DCM[0][2] = 2*(q1*q3-q0*q2);
|
||
DCM[1][0] = 2*(q1*q2 - q0*q3);
|
||
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
|
||
DCM[1][2] = 2*(q2*q3 + q0*q1);
|
||
DCM[2][0] = 2*(q1*q3 + q0*q2);
|
||
DCM[2][1] = 2*(q2*q3 - q0*q1);
|
||
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
||
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
|
||
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
|
||
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
|
||
|
||
// scale magnetometer observation error with total angular rate
|
||
R_MAG = sq(constrain_float(frontend._magNoise, 0.01f, 0.5f)) + sq(frontend.magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg);
|
||
|
||
// calculate observation jacobians
|
||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||
SH_MAG[3] = 2*q0*q1 + 2*q2*q3;
|
||
SH_MAG[4] = 2*q0*q3 + 2*q1*q2;
|
||
SH_MAG[5] = 2*q0*q2 + 2*q1*q3;
|
||
SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3);
|
||
SH_MAG[7] = 2*q1*q3 - 2*q0*q2;
|
||
SH_MAG[8] = 2*q0*q3;
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
||
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
|
||
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||
H_MAG[16] = SH_MAG[1];
|
||
H_MAG[17] = SH_MAG[4];
|
||
H_MAG[18] = SH_MAG[7];
|
||
H_MAG[19] = 1;
|
||
|
||
// calculate Kalman gain
|
||
varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))));
|
||
if (varInnovMag[0] >= R_MAG) {
|
||
SK_MX[0] = 1.0f / varInnovMag[0];
|
||
faultStatus.bad_xmag = false;
|
||
} else {
|
||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||
// we reset the covariance matrix and try again next measurement
|
||
CovarianceInit();
|
||
obsIndex = 1;
|
||
faultStatus.bad_xmag = true;
|
||
return;
|
||
}
|
||
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||
SK_MX[3] = SH_MAG[7];
|
||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
|
||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
|
||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
|
||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
|
||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
|
||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
|
||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
|
||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
|
||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
|
||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
|
||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
|
||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
|
||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
|
||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
|
||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
|
||
// this term has been zeroed to improve stability of the Z accel bias
|
||
Kfusion[15] = 0.0f;//SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
|
||
// zero Kalman gains to inhibit wind state estimation
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
|
||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
// zero Kalman gains to inhibit magnetic field state estimation
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
|
||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
|
||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
|
||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
|
||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
|
||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
|
||
} else {
|
||
for (uint8_t i=16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// reset the observation index to 0 (we start by fusing the X measurement)
|
||
obsIndex = 0;
|
||
|
||
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
|
||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||
magFusePerformed = true;
|
||
magFuseRequired = true;
|
||
}
|
||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||
{
|
||
// calculate observation jacobians
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
||
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
|
||
H_MAG[16] = 2*q1*q2 - SH_MAG[8];
|
||
H_MAG[17] = SH_MAG[0];
|
||
H_MAG[18] = SH_MAG[3];
|
||
H_MAG[20] = 1;
|
||
|
||
// calculate Kalman gain
|
||
varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2)));
|
||
if (varInnovMag[1] >= R_MAG) {
|
||
SK_MY[0] = 1.0f / varInnovMag[1];
|
||
faultStatus.bad_ymag = false;
|
||
} else {
|
||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||
// we reset the covariance matrix and try again next measurement
|
||
CovarianceInit();
|
||
obsIndex = 2;
|
||
faultStatus.bad_ymag = true;
|
||
return;
|
||
}
|
||
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||
SK_MY[3] = SH_MAG[8] - 2*q1*q2;
|
||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
|
||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
|
||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
|
||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
|
||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
|
||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
|
||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
|
||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
|
||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
|
||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
|
||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
|
||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
|
||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
|
||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
|
||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
|
||
// this term has been zeroed to improve stability of the Z accel bias
|
||
Kfusion[15] = 0.0f;//SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
|
||
// zero Kalman gains to inhibit wind state estimation
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
|
||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
// zero Kalman gains to inhibit magnetic field state estimation
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
|
||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
|
||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
|
||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
|
||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
|
||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
|
||
} else {
|
||
for (uint8_t i=16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// set flags to indicate to other processes that fusion has been performede and is required on the next frame
|
||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||
magFusePerformed = true;
|
||
magFuseRequired = true;
|
||
}
|
||
else if (obsIndex == 2) // we are now fusing the Z measurement
|
||
{
|
||
// calculate observation jacobians
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
|
||
H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
|
||
H_MAG[1] = magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3);
|
||
H_MAG[16] = SH_MAG[5];
|
||
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||
H_MAG[18] = SH_MAG[2];
|
||
H_MAG[21] = 1;
|
||
|
||
// calculate Kalman gain
|
||
varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - P[17][16]*(2*q0*q1 - 2*q2*q3) + P[1][16]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - P[17][18]*(2*q0*q1 - 2*q2*q3) + P[1][18]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - P[17][0]*(2*q0*q1 - 2*q2*q3) + P[1][0]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))) - P[17][21]*(2*q0*q1 - 2*q2*q3) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - P[17][17]*(2*q0*q1 - 2*q2*q3) + P[1][17]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))) + P[1][21]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3)) + (magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - P[17][1]*(2*q0*q1 - 2*q2*q3) + P[1][1]*(magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3))));
|
||
if (varInnovMag[2] >= R_MAG) {
|
||
SK_MZ[0] = 1.0f / varInnovMag[2];
|
||
faultStatus.bad_zmag = false;
|
||
} else {
|
||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||
// we reset the covariance matrix and try again next measurement
|
||
CovarianceInit();
|
||
obsIndex = 3;
|
||
faultStatus.bad_zmag = true;
|
||
return;
|
||
}
|
||
SK_MZ[1] = magE*SH_MAG[4] + magN*SH_MAG[1] - magD*(2*q0*q2 - 2*q1*q3);
|
||
SK_MZ[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||
SK_MZ[3] = 2*q0*q1 - 2*q2*q3;
|
||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[2] + P[0][1]*SK_MZ[1] - P[0][17]*SK_MZ[3]);
|
||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[2] + P[1][1]*SK_MZ[1] - P[1][17]*SK_MZ[3]);
|
||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[2] + P[2][1]*SK_MZ[1] - P[2][17]*SK_MZ[3]);
|
||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[2] + P[3][1]*SK_MZ[1] - P[3][17]*SK_MZ[3]);
|
||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[2] + P[4][1]*SK_MZ[1] - P[4][17]*SK_MZ[3]);
|
||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[2] + P[5][1]*SK_MZ[1] - P[5][17]*SK_MZ[3]);
|
||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[2] + P[6][1]*SK_MZ[1] - P[6][17]*SK_MZ[3]);
|
||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[2] + P[7][1]*SK_MZ[1] - P[7][17]*SK_MZ[3]);
|
||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[2] + P[8][1]*SK_MZ[1] - P[8][17]*SK_MZ[3]);
|
||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[2] + P[9][1]*SK_MZ[1] - P[9][17]*SK_MZ[3]);
|
||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[2] + P[10][1]*SK_MZ[1] - P[10][17]*SK_MZ[3]);
|
||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[2] + P[11][1]*SK_MZ[1] - P[11][17]*SK_MZ[3]);
|
||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[2] + P[12][1]*SK_MZ[1] - P[12][17]*SK_MZ[3]);
|
||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[2] + P[13][1]*SK_MZ[1] - P[13][17]*SK_MZ[3]);
|
||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[2] + P[14][1]*SK_MZ[1] - P[14][17]*SK_MZ[3]);
|
||
// this term has been zeroed to improve stability of the Z accel bias
|
||
Kfusion[15] = 0.0f;//SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[2] + P[15][1]*SK_MZ[1] - P[15][17]*SK_MZ[3]);
|
||
// zero Kalman gains to inhibit wind state estimation
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[2] + P[22][1]*SK_MZ[1] - P[22][17]*SK_MZ[3]);
|
||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[2] + P[23][1]*SK_MZ[1] - P[23][17]*SK_MZ[3]);
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
// zero Kalman gains to inhibit magnetic field state estimation
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[2] + P[16][1]*SK_MZ[1] - P[16][17]*SK_MZ[3]);
|
||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[2] + P[17][1]*SK_MZ[1] - P[17][17]*SK_MZ[3]);
|
||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[2] + P[18][1]*SK_MZ[1] - P[18][17]*SK_MZ[3]);
|
||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[2] + P[19][1]*SK_MZ[1] - P[19][17]*SK_MZ[3]);
|
||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[2] + P[20][1]*SK_MZ[1] - P[20][17]*SK_MZ[3]);
|
||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[2] + P[21][1]*SK_MZ[1] - P[21][17]*SK_MZ[3]);
|
||
} else {
|
||
for (uint8_t i=16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// set flags to indicate to other processes that fusion has been performede and is required on the next frame
|
||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||
magFusePerformed = true;
|
||
magFuseRequired = false;
|
||
}
|
||
// calculate the measurement innovation
|
||
innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
|
||
// calculate the innovation test ratio
|
||
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(frontend._magInnovGate) * varInnovMag[obsIndex]);
|
||
// check the last values from all components and set magnetometer health accordingly
|
||
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
|
||
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
|
||
// In this case we might as well try using the magnetometer, but with a reduced weighting
|
||
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) {
|
||
|
||
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
|
||
stateStruct.angErr.zero();
|
||
|
||
// correct the state vector
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||
// If we are forced to use a bad compass in flight, we reduce the weighting by a factor of 4
|
||
if (!magHealth && !constPosMode) {
|
||
Kfusion[j] *= 0.25f;
|
||
}
|
||
// If in the air and there is no other form of heading reference or we are yawing rapidly which creates larger inertial yaw errors,
|
||
// we strengthen the magnetometer attitude correction
|
||
if (filterArmed && (constPosMode || highYawRate) && j <= 3) {
|
||
Kfusion[j] *= 4.0f;
|
||
}
|
||
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion on the current time step
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// correct the covariance P = (I - K*H)*P
|
||
// take advantage of the empty columns in KH to reduce the
|
||
// number of operations
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=2; j++) {
|
||
KH[i][j] = Kfusion[i] * H_MAG[j];
|
||
}
|
||
for (uint8_t j = 3; j<=15; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
for (uint8_t j = 16; j<=21; j++) {
|
||
if (!inhibitMagStates) {
|
||
KH[i][j] = Kfusion[i] * H_MAG[j];
|
||
} else {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
}
|
||
for (uint8_t j = 22; j<=23; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
KHP[i][j] = 0;
|
||
for (uint8_t k = 0; k<=2; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
if (!inhibitMagStates) {
|
||
for (uint8_t k = 16; k<=21; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
P[i][j] = P[i][j] - KHP[i][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
// force the covariance matrix to be symmetrical and limit the variances to prevent
|
||
// ill-condiioning.
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
}
|
||
|
||
// fuse true airspeed measurements
|
||
void NavEKF2_core::FuseAirspeed()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_FuseAirspeed);
|
||
|
||
// declarations
|
||
float vn;
|
||
float ve;
|
||
float vd;
|
||
float vwn;
|
||
float vwe;
|
||
float EAS2TAS = _ahrs->get_EAS2TAS();
|
||
const float R_TAS = sq(constrain_float(frontend._easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
|
||
Vector3 SH_TAS;
|
||
float SK_TAS;
|
||
Vector24 H_TAS;
|
||
float VtasPred;
|
||
|
||
// health is set bad until test passed
|
||
tasHealth = false;
|
||
|
||
// copy required states to local variable names
|
||
vn = stateStruct.velocity.x;
|
||
ve = stateStruct.velocity.y;
|
||
vd = stateStruct.velocity.z;
|
||
vwn = stateStruct.wind_vel.x;
|
||
vwe = stateStruct.wind_vel.y;
|
||
|
||
// calculate the predicted airspeed, compensating for bias in GPS velocity when we are pulling a glitch offset back in
|
||
VtasPred = pythagorous3((ve - gpsVelGlitchOffset.y - vwe) , (vn - gpsVelGlitchOffset.x - vwn) , vd);
|
||
// perform fusion of True Airspeed measurement
|
||
if (VtasPred > 1.0f)
|
||
{
|
||
// calculate observation jacobians
|
||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||
for (uint8_t i=0; i<=2; i++) H_TAS[i] = 0.0f;
|
||
H_TAS[3] = SH_TAS[2];
|
||
H_TAS[4] = SH_TAS[1];
|
||
H_TAS[5] = vd*SH_TAS[0];
|
||
H_TAS[22] = -SH_TAS[2];
|
||
H_TAS[23] = -SH_TAS[1];
|
||
for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f;
|
||
// calculate Kalman gains
|
||
float temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]));
|
||
if (temp >= R_TAS) {
|
||
SK_TAS = 1.0f / temp;
|
||
faultStatus.bad_airspeed = false;
|
||
} else {
|
||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||
// we reset the covariance matrix and try again next measurement
|
||
CovarianceInit();
|
||
faultStatus.bad_airspeed = true;
|
||
return;
|
||
}
|
||
Kfusion[0] = SK_TAS*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SH_TAS[1] - P[0][23]*SH_TAS[1] + P[0][5]*vd*SH_TAS[0]);
|
||
Kfusion[1] = SK_TAS*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SH_TAS[1] - P[1][23]*SH_TAS[1] + P[1][5]*vd*SH_TAS[0]);
|
||
Kfusion[2] = SK_TAS*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SH_TAS[1] - P[2][23]*SH_TAS[1] + P[2][5]*vd*SH_TAS[0]);
|
||
Kfusion[3] = SK_TAS*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SH_TAS[1] - P[3][23]*SH_TAS[1] + P[3][5]*vd*SH_TAS[0]);
|
||
Kfusion[4] = SK_TAS*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[4][23]*SH_TAS[1] + P[4][5]*vd*SH_TAS[0]);
|
||
Kfusion[5] = SK_TAS*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[5][23]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]);
|
||
Kfusion[6] = SK_TAS*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SH_TAS[1] - P[6][23]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]);
|
||
Kfusion[7] = SK_TAS*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SH_TAS[1] - P[7][23]*SH_TAS[1] + P[7][5]*vd*SH_TAS[0]);
|
||
Kfusion[8] = SK_TAS*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SH_TAS[1] - P[8][23]*SH_TAS[1] + P[8][5]*vd*SH_TAS[0]);
|
||
Kfusion[9] = SK_TAS*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SH_TAS[1] - P[9][23]*SH_TAS[1] + P[9][5]*vd*SH_TAS[0]);
|
||
Kfusion[10] = SK_TAS*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SH_TAS[1] - P[10][23]*SH_TAS[1] + P[10][5]*vd*SH_TAS[0]);
|
||
Kfusion[11] = SK_TAS*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SH_TAS[1] - P[11][23]*SH_TAS[1] + P[11][5]*vd*SH_TAS[0]);
|
||
Kfusion[12] = SK_TAS*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SH_TAS[1] - P[12][23]*SH_TAS[1] + P[12][5]*vd*SH_TAS[0]);
|
||
Kfusion[13] = SK_TAS*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SH_TAS[1] - P[13][23]*SH_TAS[1] + P[13][5]*vd*SH_TAS[0]);
|
||
Kfusion[14] = SK_TAS*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SH_TAS[1] - P[14][23]*SH_TAS[1] + P[14][5]*vd*SH_TAS[0]);
|
||
// this term has been zeroed to improve stability of the Z accel bias
|
||
Kfusion[15] = 0.0f;// SK_TAS*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SH_TAS[1] - P[15][23]*SH_TAS[1] + P[15][5]*vd*SH_TAS[0]);
|
||
Kfusion[22] = SK_TAS*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SH_TAS[1] - P[22][23]*SH_TAS[1] + P[22][5]*vd*SH_TAS[0]);
|
||
Kfusion[23] = SK_TAS*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SH_TAS[1] - P[23][23]*SH_TAS[1] + P[23][5]*vd*SH_TAS[0]);
|
||
// zero Kalman gains to inhibit magnetic field state estimation
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = SK_TAS*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SH_TAS[1] - P[16][23]*SH_TAS[1] + P[16][5]*vd*SH_TAS[0]);
|
||
Kfusion[17] = SK_TAS*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SH_TAS[1] - P[17][23]*SH_TAS[1] + P[17][5]*vd*SH_TAS[0]);
|
||
Kfusion[18] = SK_TAS*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SH_TAS[1] - P[18][23]*SH_TAS[1] + P[18][5]*vd*SH_TAS[0]);
|
||
Kfusion[19] = SK_TAS*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SH_TAS[1] - P[19][23]*SH_TAS[1] + P[19][5]*vd*SH_TAS[0]);
|
||
Kfusion[20] = SK_TAS*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SH_TAS[1] - P[20][23]*SH_TAS[1] + P[20][5]*vd*SH_TAS[0]);
|
||
Kfusion[21] = SK_TAS*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SH_TAS[1] - P[21][23]*SH_TAS[1] + P[21][5]*vd*SH_TAS[0]);
|
||
} else {
|
||
for (uint8_t i=16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// calculate measurement innovation variance
|
||
varInnovVtas = 1.0f/SK_TAS;
|
||
|
||
// calculate measurement innovation
|
||
innovVtas = VtasPred - tasDataDelayed.tas;
|
||
|
||
// calculate the innovation consistency test ratio
|
||
tasTestRatio = sq(innovVtas) / (sq(frontend._tasInnovGate) * varInnovVtas);
|
||
|
||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
|
||
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend.tasRetryTime_ms;
|
||
|
||
// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
|
||
if (tasHealth || (tasTimeout && posTimeout)) {
|
||
|
||
// restart the counter
|
||
lastTasPassTime_ms = imuSampleTime_ms;
|
||
|
||
|
||
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
|
||
stateStruct.angErr.zero();
|
||
|
||
// correct the state vector
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||
statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas;
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion on the current time step
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// correct the covariance P = (I - K*H)*P
|
||
// take advantage of the empty columns in KH to reduce the
|
||
// number of operations
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=2; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
for (uint8_t j = 3; j<=5; j++) {
|
||
KH[i][j] = Kfusion[i] * H_TAS[j];
|
||
}
|
||
for (uint8_t j = 6; j<=21; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
for (uint8_t j = 22; j<=23; j++) {
|
||
KH[i][j] = Kfusion[i] * H_TAS[j];
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
KHP[i][j] = 0;
|
||
for (uint8_t k = 3; k<=5; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
for (uint8_t k = 22; k<=23; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
P[i][j] = P[i][j] - KHP[i][j];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning.
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
|
||
// stop performance timer
|
||
perf_end(_perf_FuseAirspeed);
|
||
}
|
||
|
||
// fuse sythetic sideslip measurement of zero
|
||
void NavEKF2_core::FuseSideslip()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_FuseSideslip);
|
||
|
||
// declarations
|
||
float q0;
|
||
float q1;
|
||
float q2;
|
||
float q3;
|
||
float vn;
|
||
float ve;
|
||
float vd;
|
||
float vwn;
|
||
float vwe;
|
||
const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
|
||
Vector10 SH_BETA;
|
||
Vector5 SK_BETA;
|
||
Vector3f vel_rel_wind;
|
||
Vector24 H_BETA;
|
||
float innovBeta;
|
||
|
||
// copy required states to local variable names
|
||
q0 = stateStruct.quat[0];
|
||
q1 = stateStruct.quat[1];
|
||
q2 = stateStruct.quat[2];
|
||
q3 = stateStruct.quat[3];
|
||
vn = stateStruct.velocity.x;
|
||
ve = stateStruct.velocity.y;
|
||
vd = stateStruct.velocity.z;
|
||
vwn = stateStruct.wind_vel.x;
|
||
vwe = stateStruct.wind_vel.y;
|
||
|
||
// calculate predicted wind relative velocity in NED, compensating for offset in velcity when we are pulling a GPS glitch offset back in
|
||
vel_rel_wind.x = vn - vwn - gpsVelGlitchOffset.x;
|
||
vel_rel_wind.y = ve - vwe - gpsVelGlitchOffset.y;
|
||
vel_rel_wind.z = vd;
|
||
|
||
// rotate into body axes
|
||
vel_rel_wind = prevTnb * vel_rel_wind;
|
||
|
||
// perform fusion of assumed sideslip = 0
|
||
if (vel_rel_wind.x > 5.0f)
|
||
{
|
||
// Calculate observation jacobians
|
||
SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
|
||
if (fabsf(SH_BETA[0]) <= 1e-9f) {
|
||
faultStatus.bad_sideslip = true;
|
||
return;
|
||
} else {
|
||
faultStatus.bad_sideslip = false;
|
||
}
|
||
SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
|
||
SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
|
||
SH_BETA[2] = vd*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) - (ve - vwe)*(2*q0*q1 - 2*q2*q3) + (vn - vwn)*(2*q0*q2 + 2*q1*q3);
|
||
SH_BETA[3] = 1/sq(SH_BETA[0]);
|
||
SH_BETA[4] = (sq(q0) - sq(q1) + sq(q2) - sq(q3))/SH_BETA[0];
|
||
SH_BETA[5] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||
SH_BETA[6] = 1/SH_BETA[0];
|
||
SH_BETA[7] = 2*q0*q3;
|
||
SH_BETA[8] = SH_BETA[7] + 2*q1*q2;
|
||
SH_BETA[9] = SH_BETA[7] - 2*q1*q2;
|
||
H_BETA[0] = SH_BETA[2]*SH_BETA[6];
|
||
H_BETA[1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[3];
|
||
H_BETA[2] = - sq(SH_BETA[1])*SH_BETA[3] - 1;
|
||
H_BETA[3] = - SH_BETA[6]*SH_BETA[9] - SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||
H_BETA[4] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
|
||
H_BETA[5] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
|
||
for (uint8_t i=6; i<=21; i++) {
|
||
H_BETA[i] = 0.0f;
|
||
}
|
||
H_BETA[22] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||
H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4];
|
||
|
||
// Calculate Kalman gains
|
||
float temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]));
|
||
if (temp >= R_BETA) {
|
||
SK_BETA[0] = 1.0f / temp;
|
||
faultStatus.bad_sideslip = false;
|
||
} else {
|
||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||
// we reset the covariance matrix and try again next measurement
|
||
CovarianceInit();
|
||
faultStatus.bad_sideslip = true;
|
||
return;
|
||
}
|
||
SK_BETA[1] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
|
||
SK_BETA[2] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
|
||
SK_BETA[3] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
|
||
SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[3] + 1;
|
||
Kfusion[0] = SK_BETA[0]*(P[0][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[6]*SH_BETA[2] + P[0][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[6]*SH_BETA[2] + P[1][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[6]*SH_BETA[2] + P[2][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[6]*SH_BETA[2] + P[3][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[6]*SH_BETA[2] + P[4][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[6]*SH_BETA[2] + P[5][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[6]*SH_BETA[2] + P[6][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[6]*SH_BETA[2] + P[7][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[6]*SH_BETA[2] + P[8][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[6]*SH_BETA[2] + P[9][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[6]*SH_BETA[2] + P[10][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[6]*SH_BETA[2] + P[11][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[6]*SH_BETA[2] + P[12][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[6]*SH_BETA[2] + P[13][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[6]*SH_BETA[2] + P[14][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
// this term has been zeroed to improve stability of the Z accel bias
|
||
Kfusion[15] = 0.0f;//SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[6]*SH_BETA[2] + P[15][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[6]*SH_BETA[2] + P[22][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[6]*SH_BETA[2] + P[23][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
// zero Kalman gains to inhibit magnetic field state estimation
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[6]*SH_BETA[2] + P[16][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[6]*SH_BETA[2] + P[17][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[6]*SH_BETA[2] + P[18][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[6]*SH_BETA[2] + P[19][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[6]*SH_BETA[2] + P[20][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[6]*SH_BETA[2] + P[21][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
|
||
} else {
|
||
for (uint8_t i=16; i<=21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
// calculate predicted sideslip angle and innovation using small angle approximation
|
||
innovBeta = vel_rel_wind.y / vel_rel_wind.x;
|
||
|
||
// reject measurement if greater than 3-sigma inconsistency
|
||
if (innovBeta > 0.5f) {
|
||
return;
|
||
}
|
||
|
||
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
|
||
stateStruct.angErr.zero();
|
||
|
||
// correct the state vector
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||
statesArray[j] = statesArray[j] - Kfusion[j] * innovBeta;
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion on the current time step
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// correct the covariance P = (I - K*H)*P
|
||
// take advantage of the empty columns in KH to reduce the
|
||
// number of operations
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=5; j++) {
|
||
KH[i][j] = Kfusion[i] * H_BETA[j];
|
||
}
|
||
for (uint8_t j = 6; j<=21; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
for (uint8_t j = 22; j<=23; j++) {
|
||
KH[i][j] = Kfusion[i] * H_BETA[j];
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
KHP[i][j] = 0;
|
||
for (uint8_t k = 0; k<=5; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
for (uint8_t k = 22; k<=23; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
P[i][j] = P[i][j] - KHP[i][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
// force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning.
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
|
||
// stop the performance timer
|
||
perf_end(_perf_FuseSideslip);
|
||
}
|
||
|
||
/*
|
||
Fuse angular motion compensated optical flow rates into the main filter.
|
||
Requires a valid terrain height estimate.
|
||
*/
|
||
void NavEKF2_core::FuseOptFlow()
|
||
{
|
||
Vector24 H_LOS;
|
||
Vector3f velNED_local;
|
||
Vector3f relVelSensor;
|
||
Vector14 SH_LOS;
|
||
Vector2 losPred;
|
||
|
||
// Copy required states to local variable names
|
||
float q0 = stateStruct.quat[0];
|
||
float q1 = stateStruct.quat[1];
|
||
float q2 = stateStruct.quat[2];
|
||
float q3 = stateStruct.quat[3];
|
||
float vn = stateStruct.velocity.x;
|
||
float ve = stateStruct.velocity.y;
|
||
float vd = stateStruct.velocity.z;
|
||
float pd = stateStruct.position.z;
|
||
|
||
// Correct velocities for GPS glitch recovery offset
|
||
velNED_local.x = vn - gpsVelGlitchOffset.x;
|
||
velNED_local.y = ve - gpsVelGlitchOffset.y;
|
||
velNED_local.z = vd;
|
||
|
||
// constrain height above ground to be above range measured on ground
|
||
float heightAboveGndEst = max((terrainState - pd), rngOnGnd);
|
||
float ptd = pd + heightAboveGndEst;
|
||
|
||
// Calculate common expressions for observation jacobians
|
||
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
|
||
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
|
||
SH_LOS[3] = 1/(pd - ptd);
|
||
SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
|
||
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||
SH_LOS[7] = q0*q0;
|
||
SH_LOS[8] = q1*q1;
|
||
SH_LOS[9] = q2*q2;
|
||
SH_LOS[10] = q3*q3;
|
||
SH_LOS[11] = q0*q3*2.0f;
|
||
SH_LOS[12] = pd-ptd;
|
||
SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
|
||
|
||
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
|
||
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
|
||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||
float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f);
|
||
|
||
// calculate relative velocity in sensor frame
|
||
relVelSensor = Tnb_flow*velNED_local;
|
||
|
||
// divide velocity by range to get predicted angular LOS rates relative to X and Y axes
|
||
losPred[0] = relVelSensor.y/range;
|
||
losPred[1] = -relVelSensor.x/range;
|
||
|
||
// calculate observation jacobians and Kalman gains
|
||
memset(&H_LOS[0], 0, sizeof(H_LOS));
|
||
if (obsIndex == 0) {
|
||
H_LOS[0] = SH_LOS[3]*SH_LOS[2]*SH_LOS[6]-SH_LOS[3]*SH_LOS[0]*SH_LOS[4];
|
||
H_LOS[1] = SH_LOS[3]*SH_LOS[2]*SH_LOS[5];
|
||
H_LOS[2] = SH_LOS[3]*SH_LOS[0]*SH_LOS[1];
|
||
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]-q1*q2*2.0f);
|
||
H_LOS[4] = -SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]-SH_LOS[8]+SH_LOS[9]-SH_LOS[10]);
|
||
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
|
||
H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
|
||
|
||
float t2 = SH_LOS[3];
|
||
float t3 = SH_LOS[0];
|
||
float t4 = SH_LOS[2];
|
||
float t5 = SH_LOS[6];
|
||
float t100 = t2 * t3 * t5;
|
||
float t6 = SH_LOS[4];
|
||
float t7 = t2*t3*t6;
|
||
float t9 = t2*t4*t5;
|
||
float t8 = t7-t9;
|
||
float t10 = q0*q3*2.0f;
|
||
float t21 = q1*q2*2.0f;
|
||
float t11 = t10-t21;
|
||
float t101 = t2 * t3 * t11;
|
||
float t12 = pd-ptd;
|
||
float t13 = 1.0f/(t12*t12);
|
||
float t104 = t3 * t4 * t13;
|
||
float t14 = SH_LOS[5];
|
||
float t102 = t2 * t4 * t14;
|
||
float t15 = SH_LOS[1];
|
||
float t103 = t2 * t3 * t15;
|
||
float t16 = q0*q0;
|
||
float t17 = q1*q1;
|
||
float t18 = q2*q2;
|
||
float t19 = q3*q3;
|
||
float t20 = t16-t17+t18-t19;
|
||
float t105 = t2 * t3 * t20;
|
||
float t22 = P[1][1]*t102;
|
||
float t23 = P[3][0]*t101;
|
||
float t24 = P[8][0]*t104;
|
||
float t25 = P[1][0]*t102;
|
||
float t26 = P[2][0]*t103;
|
||
float t63 = P[0][0]*t8;
|
||
float t64 = P[5][0]*t100;
|
||
float t65 = P[4][0]*t105;
|
||
float t27 = t23+t24+t25+t26-t63-t64-t65;
|
||
float t28 = P[3][3]*t101;
|
||
float t29 = P[8][3]*t104;
|
||
float t30 = P[1][3]*t102;
|
||
float t31 = P[2][3]*t103;
|
||
float t67 = P[0][3]*t8;
|
||
float t68 = P[5][3]*t100;
|
||
float t69 = P[4][3]*t105;
|
||
float t32 = t28+t29+t30+t31-t67-t68-t69;
|
||
float t33 = t101*t32;
|
||
float t34 = P[3][8]*t101;
|
||
float t35 = P[8][8]*t104;
|
||
float t36 = P[1][8]*t102;
|
||
float t37 = P[2][8]*t103;
|
||
float t70 = P[0][8]*t8;
|
||
float t71 = P[5][8]*t100;
|
||
float t72 = P[4][8]*t105;
|
||
float t38 = t34+t35+t36+t37-t70-t71-t72;
|
||
float t39 = t104*t38;
|
||
float t40 = P[3][1]*t101;
|
||
float t41 = P[8][1]*t104;
|
||
float t42 = P[2][1]*t103;
|
||
float t73 = P[0][1]*t8;
|
||
float t74 = P[5][1]*t100;
|
||
float t75 = P[4][1]*t105;
|
||
float t43 = t22+t40+t41+t42-t73-t74-t75;
|
||
float t44 = t102*t43;
|
||
float t45 = P[3][2]*t101;
|
||
float t46 = P[8][2]*t104;
|
||
float t47 = P[1][2]*t102;
|
||
float t48 = P[2][2]*t103;
|
||
float t76 = P[0][2]*t8;
|
||
float t77 = P[5][2]*t100;
|
||
float t78 = P[4][2]*t105;
|
||
float t49 = t45+t46+t47+t48-t76-t77-t78;
|
||
float t50 = t103*t49;
|
||
float t51 = P[3][5]*t101;
|
||
float t52 = P[8][5]*t104;
|
||
float t53 = P[1][5]*t102;
|
||
float t54 = P[2][5]*t103;
|
||
float t79 = P[0][5]*t8;
|
||
float t80 = P[5][5]*t100;
|
||
float t81 = P[4][5]*t105;
|
||
float t55 = t51+t52+t53+t54-t79-t80-t81;
|
||
float t56 = P[3][4]*t101;
|
||
float t57 = P[8][4]*t104;
|
||
float t58 = P[1][4]*t102;
|
||
float t59 = P[2][4]*t103;
|
||
float t83 = P[0][4]*t8;
|
||
float t84 = P[5][4]*t100;
|
||
float t85 = P[4][4]*t105;
|
||
float t60 = t56+t57+t58+t59-t83-t84-t85;
|
||
float t66 = t8*t27;
|
||
float t82 = t100*t55;
|
||
float t86 = t105*t60;
|
||
float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86;
|
||
float t62 = 1.0f/t61;
|
||
|
||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||
if (t61 > R_LOS) {
|
||
t62 = 1.0f/t61;
|
||
} else {
|
||
t61 = 0.0f;
|
||
t62 = 1.0f/R_LOS;
|
||
}
|
||
varInnovOptFlow[0] = t61;
|
||
|
||
// calculate innovation for X axis observation
|
||
innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
|
||
|
||
// calculate Kalman gains for X-axis observation
|
||
Kfusion[0] = t62*(-P[0][0]*t8-P[0][5]*t100+P[0][3]*t101+P[0][1]*t102+P[0][2]*t103+P[0][8]*t104-P[0][4]*t105);
|
||
Kfusion[1] = t62*(t22-P[1][0]*t8-P[1][5]*t100+P[1][3]*t101+P[1][2]*t103+P[1][8]*t104-P[1][4]*t105);
|
||
Kfusion[2] = t62*(t48-P[2][0]*t8-P[2][5]*t100+P[2][3]*t101+P[2][1]*t102+P[2][8]*t104-P[2][4]*t105);
|
||
Kfusion[3] = t62*(t28-P[3][0]*t8-P[3][5]*t100+P[3][1]*t102+P[3][2]*t103+P[3][8]*t104-P[3][4]*t105);
|
||
Kfusion[4] = t62*(-t85-P[4][0]*t8-P[4][5]*t100+P[4][3]*t101+P[4][1]*t102+P[4][2]*t103+P[4][8]*t104);
|
||
Kfusion[5] = t62*(-t80-P[5][0]*t8+P[5][3]*t101+P[5][1]*t102+P[5][2]*t103+P[5][8]*t104-P[5][4]*t105);
|
||
Kfusion[6] = t62*(-P[6][0]*t8-P[6][5]*t100+P[6][3]*t101+P[6][1]*t102+P[6][2]*t103+P[6][8]*t104-P[6][4]*t105);
|
||
Kfusion[7] = t62*(-P[7][0]*t8-P[7][5]*t100+P[7][3]*t101+P[7][1]*t102+P[7][2]*t103+P[7][8]*t104-P[7][4]*t105);
|
||
Kfusion[8] = t62*(t35-P[8][0]*t8-P[8][5]*t100+P[8][3]*t101+P[8][1]*t102+P[8][2]*t103-P[8][4]*t105);
|
||
Kfusion[9] = t62*(-P[9][0]*t8-P[9][5]*t100+P[9][3]*t101+P[9][1]*t102+P[9][2]*t103+P[9][8]*t104-P[9][4]*t105);
|
||
Kfusion[10] = t62*(-P[10][0]*t8-P[10][5]*t100+P[10][3]*t101+P[10][1]*t102+P[10][2]*t103+P[10][8]*t104-P[10][4]*t105);
|
||
Kfusion[11] = t62*(-P[11][0]*t8-P[11][5]*t100+P[11][3]*t101+P[11][1]*t102+P[11][2]*t103+P[11][8]*t104-P[11][4]*t105);
|
||
Kfusion[12] = t62*(-P[12][0]*t8-P[12][5]*t100+P[12][3]*t101+P[12][1]*t102+P[12][2]*t103+P[12][8]*t104-P[12][4]*t105);
|
||
Kfusion[13] = t62*(-P[13][0]*t8-P[13][5]*t100+P[13][3]*t101+P[13][1]*t102+P[13][2]*t103+P[13][8]*t104-P[13][4]*t105);
|
||
Kfusion[14] = t62*(-P[14][0]*t8-P[14][5]*t100+P[14][3]*t101+P[14][1]*t102+P[14][2]*t103+P[14][8]*t104-P[14][4]*t105);
|
||
Kfusion[15] = t62*(-P[15][0]*t8-P[15][5]*t100+P[15][3]*t101+P[15][1]*t102+P[15][2]*t103+P[15][8]*t104-P[15][4]*t105);
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = t62*(-P[22][0]*t8-P[22][5]*t100+P[22][3]*t101+P[22][1]*t102+P[22][2]*t103+P[22][8]*t104-P[22][4]*t105);
|
||
Kfusion[23] = t62*(-P[23][0]*t8-P[23][5]*t100+P[23][3]*t101+P[23][1]*t102+P[23][2]*t103+P[23][8]*t104-P[23][4]*t105);
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = t62*(-P[16][0]*t8-P[16][5]*t100+P[16][3]*t101+P[16][1]*t102+P[16][2]*t103+P[16][8]*t104-P[16][4]*t105);
|
||
Kfusion[17] = t62*(-P[17][0]*t8-P[17][5]*t100+P[17][3]*t101+P[17][1]*t102+P[17][2]*t103+P[17][8]*t104-P[17][4]*t105);
|
||
Kfusion[18] = t62*(-P[18][0]*t8-P[18][5]*t100+P[18][3]*t101+P[18][1]*t102+P[18][2]*t103+P[18][8]*t104-P[18][4]*t105);
|
||
Kfusion[19] = t62*(-P[19][0]*t8-P[19][5]*t100+P[19][3]*t101+P[19][1]*t102+P[19][2]*t103+P[19][8]*t104-P[19][4]*t105);
|
||
Kfusion[20] = t62*(-P[20][0]*t8-P[20][5]*t100+P[20][3]*t101+P[20][1]*t102+P[20][2]*t103+P[20][8]*t104-P[20][4]*t105);
|
||
Kfusion[21] = t62*(-P[21][0]*t8-P[21][5]*t100+P[21][3]*t101+P[21][1]*t102+P[21][2]*t103+P[21][8]*t104-P[21][4]*t105);
|
||
} else {
|
||
for (uint8_t i = 16; i <= 21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
|
||
} else {
|
||
|
||
H_LOS[0] = -SH_LOS[3]*SH_LOS[6]*SH_LOS[1];
|
||
H_LOS[1] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[4]-SH_LOS[3]*SH_LOS[1]*SH_LOS[5];
|
||
H_LOS[2] = SH_LOS[3]*SH_LOS[2]*SH_LOS[0];
|
||
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]+SH_LOS[8]-SH_LOS[9]-SH_LOS[10]);
|
||
H_LOS[4] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]+q1*q2*2.0f);
|
||
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
|
||
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
|
||
|
||
float t2 = SH_LOS[3];
|
||
float t3 = SH_LOS[0];
|
||
float t4 = SH_LOS[1];
|
||
float t5 = SH_LOS[5];
|
||
float t100 = t2 * t3 * t5;
|
||
float t6 = SH_LOS[4];
|
||
float t7 = t2*t3*t6;
|
||
float t8 = t2*t4*t5;
|
||
float t9 = t7+t8;
|
||
float t10 = q0*q3*2.0f;
|
||
float t11 = q1*q2*2.0f;
|
||
float t12 = t10+t11;
|
||
float t101 = t2 * t3 * t12;
|
||
float t13 = pd-ptd;
|
||
float t14 = 1.0f/(t13*t13);
|
||
float t104 = t3 * t4 * t14;
|
||
float t15 = SH_LOS[6];
|
||
float t105 = t2 * t4 * t15;
|
||
float t16 = SH_LOS[2];
|
||
float t102 = t2 * t3 * t16;
|
||
float t17 = q0*q0;
|
||
float t18 = q1*q1;
|
||
float t19 = q2*q2;
|
||
float t20 = q3*q3;
|
||
float t21 = t17+t18-t19-t20;
|
||
float t103 = t2 * t3 * t21;
|
||
float t22 = P[0][0]*t105;
|
||
float t23 = P[1][1]*t9;
|
||
float t24 = P[8][1]*t104;
|
||
float t25 = P[0][1]*t105;
|
||
float t26 = P[5][1]*t100;
|
||
float t64 = P[4][1]*t101;
|
||
float t65 = P[2][1]*t102;
|
||
float t66 = P[3][1]*t103;
|
||
float t27 = t23+t24+t25+t26-t64-t65-t66;
|
||
float t28 = t9*t27;
|
||
float t29 = P[1][4]*t9;
|
||
float t30 = P[8][4]*t104;
|
||
float t31 = P[0][4]*t105;
|
||
float t32 = P[5][4]*t100;
|
||
float t67 = P[4][4]*t101;
|
||
float t68 = P[2][4]*t102;
|
||
float t69 = P[3][4]*t103;
|
||
float t33 = t29+t30+t31+t32-t67-t68-t69;
|
||
float t34 = P[1][8]*t9;
|
||
float t35 = P[8][8]*t104;
|
||
float t36 = P[0][8]*t105;
|
||
float t37 = P[5][8]*t100;
|
||
float t71 = P[4][8]*t101;
|
||
float t72 = P[2][8]*t102;
|
||
float t73 = P[3][8]*t103;
|
||
float t38 = t34+t35+t36+t37-t71-t72-t73;
|
||
float t39 = t104*t38;
|
||
float t40 = P[1][0]*t9;
|
||
float t41 = P[8][0]*t104;
|
||
float t42 = P[5][0]*t100;
|
||
float t74 = P[4][0]*t101;
|
||
float t75 = P[2][0]*t102;
|
||
float t76 = P[3][0]*t103;
|
||
float t43 = t22+t40+t41+t42-t74-t75-t76;
|
||
float t44 = t105*t43;
|
||
float t45 = P[1][2]*t9;
|
||
float t46 = P[8][2]*t104;
|
||
float t47 = P[0][2]*t105;
|
||
float t48 = P[5][2]*t100;
|
||
float t63 = P[2][2]*t102;
|
||
float t77 = P[4][2]*t101;
|
||
float t78 = P[3][2]*t103;
|
||
float t49 = t45+t46+t47+t48-t63-t77-t78;
|
||
float t50 = P[1][5]*t9;
|
||
float t51 = P[8][5]*t104;
|
||
float t52 = P[0][5]*t105;
|
||
float t53 = P[5][5]*t100;
|
||
float t80 = P[4][5]*t101;
|
||
float t81 = P[2][5]*t102;
|
||
float t82 = P[3][5]*t103;
|
||
float t54 = t50+t51+t52+t53-t80-t81-t82;
|
||
float t55 = t100*t54;
|
||
float t56 = P[1][3]*t9;
|
||
float t57 = P[8][3]*t104;
|
||
float t58 = P[0][3]*t105;
|
||
float t59 = P[5][3]*t100;
|
||
float t83 = P[4][3]*t101;
|
||
float t84 = P[2][3]*t102;
|
||
float t85 = P[3][3]*t103;
|
||
float t60 = t56+t57+t58+t59-t83-t84-t85;
|
||
float t70 = t101*t33;
|
||
float t79 = t102*t49;
|
||
float t86 = t103*t60;
|
||
float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86;
|
||
float t62 = 1.0f/t61;
|
||
|
||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||
if (t61 > R_LOS) {
|
||
t62 = 1.0f/t61;
|
||
} else {
|
||
t61 = 0.0f;
|
||
t62 = 1.0f/R_LOS;
|
||
}
|
||
varInnovOptFlow[1] = t61;
|
||
|
||
// calculate innovation for Y observation
|
||
innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
|
||
|
||
// calculate Kalman gains for the Y-axis observation
|
||
Kfusion[0] = -t62*(t22+P[0][1]*t9+P[0][5]*t100-P[0][4]*t101-P[0][2]*t102-P[0][3]*t103+P[0][8]*t104);
|
||
Kfusion[1] = -t62*(t23+P[1][5]*t100+P[1][0]*t105-P[1][4]*t101-P[1][2]*t102-P[1][3]*t103+P[1][8]*t104);
|
||
Kfusion[2] = -t62*(-t63+P[2][1]*t9+P[2][5]*t100+P[2][0]*t105-P[2][4]*t101-P[2][3]*t103+P[2][8]*t104);
|
||
Kfusion[3] = -t62*(-t85+P[3][1]*t9+P[3][5]*t100+P[3][0]*t105-P[3][4]*t101-P[3][2]*t102+P[3][8]*t104);
|
||
Kfusion[4] = -t62*(-t67+P[4][1]*t9+P[4][5]*t100+P[4][0]*t105-P[4][2]*t102-P[4][3]*t103+P[4][8]*t104);
|
||
Kfusion[5] = -t62*(t53+P[5][1]*t9+P[5][0]*t105-P[5][4]*t101-P[5][2]*t102-P[5][3]*t103+P[5][8]*t104);
|
||
Kfusion[6] = -t62*(P[6][1]*t9+P[6][5]*t100+P[6][0]*t105-P[6][4]*t101-P[6][2]*t102-P[6][3]*t103+P[6][8]*t104);
|
||
Kfusion[7] = -t62*(P[7][1]*t9+P[7][5]*t100+P[7][0]*t105-P[7][4]*t101-P[7][2]*t102-P[7][3]*t103+P[7][8]*t104);
|
||
Kfusion[8] = -t62*(t35+P[8][1]*t9+P[8][5]*t100+P[8][0]*t105-P[8][4]*t101-P[8][2]*t102-P[8][3]*t103);
|
||
Kfusion[9] = -t62*(P[9][1]*t9+P[9][5]*t100+P[9][0]*t105-P[9][4]*t101-P[9][2]*t102-P[9][3]*t103+P[9][8]*t104);
|
||
Kfusion[10] = -t62*(P[10][1]*t9+P[10][5]*t100+P[10][0]*t105-P[10][4]*t101-P[10][2]*t102-P[10][3]*t103+P[10][8]*t104);
|
||
Kfusion[11] = -t62*(P[11][1]*t9+P[11][5]*t100+P[11][0]*t105-P[11][4]*t101-P[11][2]*t102-P[11][3]*t103+P[11][8]*t104);
|
||
Kfusion[12] = -t62*(P[12][1]*t9+P[12][5]*t100+P[12][0]*t105-P[12][4]*t101-P[12][2]*t102-P[12][3]*t103+P[12][8]*t104);
|
||
Kfusion[13] = -t62*(P[13][1]*t9+P[13][5]*t100+P[13][0]*t105-P[13][4]*t101-P[13][2]*t102-P[13][3]*t103+P[13][8]*t104);
|
||
Kfusion[14] = -t62*(P[14][1]*t9+P[14][5]*t100+P[14][0]*t105-P[14][4]*t101-P[14][2]*t102-P[14][3]*t103+P[14][8]*t104);
|
||
Kfusion[15] = -t62*(P[15][1]*t9+P[15][5]*t100+P[15][0]*t105-P[15][4]*t101-P[15][2]*t102-P[15][3]*t103+P[15][8]*t104);
|
||
if (!inhibitWindStates) {
|
||
Kfusion[22] = -t62*(P[22][1]*t9+P[22][5]*t100+P[22][0]*t105-P[22][4]*t101-P[22][2]*t102-P[22][3]*t103+P[22][8]*t104);
|
||
Kfusion[23] = -t62*(P[23][1]*t9+P[23][5]*t100+P[23][0]*t105-P[23][4]*t101-P[23][2]*t102-P[23][3]*t103+P[23][8]*t104);
|
||
} else {
|
||
Kfusion[22] = 0.0f;
|
||
Kfusion[23] = 0.0f;
|
||
}
|
||
if (!inhibitMagStates) {
|
||
Kfusion[16] = -t62*(P[16][1]*t9+P[16][5]*t100+P[16][0]*t105-P[16][4]*t101-P[16][2]*t102-P[16][3]*t103+P[16][8]*t104);
|
||
Kfusion[17] = -t62*(P[17][1]*t9+P[17][5]*t100+P[17][0]*t105-P[17][4]*t101-P[17][2]*t102-P[17][3]*t103+P[17][8]*t104);
|
||
Kfusion[18] = -t62*(P[18][1]*t9+P[18][5]*t100+P[18][0]*t105-P[18][4]*t101-P[18][2]*t102-P[18][3]*t103+P[18][8]*t104);
|
||
Kfusion[19] = -t62*(P[19][1]*t9+P[19][5]*t100+P[19][0]*t105-P[19][4]*t101-P[19][2]*t102-P[19][3]*t103+P[19][8]*t104);
|
||
Kfusion[20] = -t62*(P[20][1]*t9+P[20][5]*t100+P[20][0]*t105-P[20][4]*t101-P[20][2]*t102-P[20][3]*t103+P[20][8]*t104);
|
||
Kfusion[21] = -t62*(P[21][1]*t9+P[21][5]*t100+P[21][0]*t105-P[21][4]*t101-P[21][2]*t102-P[21][3]*t103+P[21][8]*t104);
|
||
} else {
|
||
for (uint8_t i = 16; i <= 21; i++) {
|
||
Kfusion[i] = 0.0f;
|
||
}
|
||
}
|
||
}
|
||
|
||
// calculate the innovation consistency test ratio
|
||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(frontend._flowInnovGate) * varInnovOptFlow[obsIndex]);
|
||
|
||
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
|
||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend._maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend._maxFlowRate)) {
|
||
// record the last time observations were accepted for fusion
|
||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||
|
||
// zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
|
||
stateStruct.angErr.zero();
|
||
|
||
// correct the state vector
|
||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||
statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion on the current time step
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// correct the covariance P = (I - K*H)*P
|
||
// take advantage of the empty columns in KH to reduce the
|
||
// number of operations
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=5; j++) {
|
||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||
}
|
||
for (uint8_t j = 6; j<=7; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
KH[i][8] = Kfusion[i] * H_LOS[8];
|
||
for (uint8_t j = 9; j<=23; j++) {
|
||
KH[i][j] = 0.0f;
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
KHP[i][j] = 0;
|
||
for (uint8_t k = 0; k<=5; k++) {
|
||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||
}
|
||
KHP[i][j] = KHP[i][j] + KH[i][8] * P[8][j];
|
||
}
|
||
}
|
||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j = 0; j<=stateIndexLim; j++) {
|
||
P[i][j] = P[i][j] - KHP[i][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
// fix basic numerical errors
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
|
||
}
|
||
}
|
||
|
||
/*
|
||
Estimation of terrain offset using a single state EKF
|
||
The filter can fuse motion compensated optiocal flow rates and range finder measurements
|
||
*/
|
||
void NavEKF2_core::EstimateTerrainOffset()
|
||
{
|
||
// start performance timer
|
||
perf_begin(_perf_OpticalFlowEKF);
|
||
|
||
// constrain height above ground to be above range measured on ground
|
||
float heightAboveGndEst = max((terrainState - stateStruct.position.z), rngOnGnd);
|
||
|
||
// calculate a predicted LOS rate squared
|
||
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
|
||
float losRateSq = velHorizSq / sq(heightAboveGndEst);
|
||
|
||
// don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
|
||
if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f || onGround)) {
|
||
inhibitGndState = true;
|
||
} else {
|
||
inhibitGndState = false;
|
||
// record the time we last updated the terrain offset state
|
||
gndHgtValidTime_ms = imuSampleTime_ms;
|
||
|
||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
|
||
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
|
||
prevPosN = stateStruct.position[0];
|
||
prevPosE = stateStruct.position[1];
|
||
|
||
// in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter
|
||
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend.gndGradientSigma))) + sq(float(frontend.gndGradientSigma) * timeLapsed);
|
||
Popt += Pincrement;
|
||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||
|
||
// fuse range finder data
|
||
if (fuseRngData) {
|
||
// predict range
|
||
float predRngMeas = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z;
|
||
|
||
// Copy required states to local variable names
|
||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||
|
||
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||
float R_RNG = frontend._rngNoise;
|
||
|
||
// calculate Kalman gain
|
||
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
|
||
|
||
// Calculate the innovation variance for data logging
|
||
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
|
||
|
||
// constrain terrain height to be below the vehicle
|
||
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
|
||
|
||
// Calculate the measurement innovation
|
||
innovRng = predRngMeas - rngMea;
|
||
|
||
// calculate the innovation consistency test ratio
|
||
auxRngTestRatio = sq(innovRng) / (sq(frontend._rngInnovGate) * varInnovRng);
|
||
|
||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||
if ((sq(innovRng)*SK_RNG) < 25.0f)
|
||
{
|
||
// correct the state
|
||
terrainState -= K_RNG * innovRng;
|
||
|
||
// constrain the state
|
||
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
|
||
|
||
// correct the covariance
|
||
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
|
||
|
||
// prevent the state variance from becoming negative
|
||
Popt = max(Popt,0.0f);
|
||
|
||
}
|
||
}
|
||
|
||
if (fuseOptFlowData) {
|
||
|
||
Vector3f vel; // velocity of sensor relative to ground in NED axes
|
||
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||
float losPred; // predicted optical flow angular rate measurement
|
||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||
float K_OPT;
|
||
float H_OPT;
|
||
|
||
// Correct velocities for GPS glitch recovery offset
|
||
vel.x = stateStruct.velocity[0] - gpsVelGlitchOffset.x;
|
||
vel.y = stateStruct.velocity[1] - gpsVelGlitchOffset.y;
|
||
vel.z = stateStruct.velocity[2];
|
||
|
||
// predict range to centre of image
|
||
float flowRngPred = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z;
|
||
|
||
// constrain terrain height to be below the vehicle
|
||
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
|
||
|
||
// calculate relative velocity in sensor frame
|
||
relVelSensor = Tnb_flow*vel;
|
||
|
||
// divide velocity by range, subtract body rates and apply scale factor to
|
||
// get predicted sensed angular optical rates relative to X and Y sensor axes
|
||
losPred = relVelSensor.length()/flowRngPred;
|
||
|
||
// calculate innovations
|
||
auxFlowObsInnov = losPred - sqrtf(sq(flowRadXYcomp[0]) + sq(flowRadXYcomp[1]));
|
||
|
||
// calculate observation jacobian
|
||
float t3 = sq(q0);
|
||
float t4 = sq(q1);
|
||
float t5 = sq(q2);
|
||
float t6 = sq(q3);
|
||
float t10 = q0*q3*2.0f;
|
||
float t11 = q1*q2*2.0f;
|
||
float t14 = t3+t4-t5-t6;
|
||
float t15 = t14*vel.x;
|
||
float t16 = t10+t11;
|
||
float t17 = t16*vel.y;
|
||
float t18 = q0*q2*2.0f;
|
||
float t19 = q1*q3*2.0f;
|
||
float t20 = t18-t19;
|
||
float t21 = t20*vel.z;
|
||
float t2 = t15+t17-t21;
|
||
float t7 = t3-t4-t5+t6;
|
||
float t8 = stateStruct.position[2]-terrainState;
|
||
float t9 = 1.0f/sq(t8);
|
||
float t24 = t3-t4+t5-t6;
|
||
float t25 = t24*vel.y;
|
||
float t26 = t10-t11;
|
||
float t27 = t26*vel.x;
|
||
float t28 = q0*q1*2.0f;
|
||
float t29 = q2*q3*2.0f;
|
||
float t30 = t28+t29;
|
||
float t31 = t30*vel.z;
|
||
float t12 = t25-t27+t31;
|
||
float t13 = sq(t7);
|
||
float t22 = sq(t2);
|
||
float t23 = 1.0f/(t8*t8*t8);
|
||
float t32 = sq(t12);
|
||
H_OPT = 0.5f*(t13*t22*t23*2.0f+t13*t23*t32*2.0f)/sqrtf(t9*t13*t22+t9*t13*t32);
|
||
|
||
// calculate innovation variances
|
||
auxFlowObsInnovVar = H_OPT*Popt*H_OPT + R_LOS;
|
||
|
||
// calculate Kalman gain
|
||
K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
|
||
|
||
// calculate the innovation consistency test ratio
|
||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend._flowInnovGate) * auxFlowObsInnovVar);
|
||
|
||
// don't fuse if optical flow data is outside valid range
|
||
if (max(flowRadXY[0],flowRadXY[1]) < frontend._maxFlowRate) {
|
||
|
||
// correct the state
|
||
terrainState -= K_OPT * auxFlowObsInnov;
|
||
|
||
// constrain the state
|
||
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
|
||
|
||
// correct the covariance
|
||
Popt = Popt - K_OPT * H_OPT * Popt;
|
||
|
||
// prevent the state variances from becoming negative
|
||
Popt = max(Popt,0.0f);
|
||
}
|
||
}
|
||
}
|
||
|
||
// stop the performance timer
|
||
perf_end(_perf_OpticalFlowEKF);
|
||
}
|
||
|
||
// zero specified range of rows in the state covariance matrix
|
||
void NavEKF2_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
|
||
{
|
||
uint8_t row;
|
||
for (row=first; row<=last; row++)
|
||
{
|
||
memset(&covMat[row][0], 0, sizeof(covMat[0][0])*24);
|
||
}
|
||
}
|
||
|
||
// zero specified range of columns in the state covariance matrix
|
||
void NavEKF2_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
|
||
{
|
||
uint8_t row;
|
||
for (row=0; row<=23; row++)
|
||
{
|
||
memset(&covMat[row][first], 0, sizeof(covMat[0][0])*(1+last-first));
|
||
}
|
||
}
|
||
|
||
// store output data in the FIFO
|
||
void NavEKF2_core::StoreOutput()
|
||
{
|
||
storedOutput[fifoIndexNow] = outputDataNew;
|
||
}
|
||
|
||
// reset the output data to the current EKF state
|
||
void NavEKF2_core::StoreOutputReset()
|
||
{
|
||
outputDataNew.quat = stateStruct.quat;
|
||
outputDataNew.velocity = stateStruct.velocity;
|
||
outputDataNew.position = stateStruct.position;
|
||
// write current measurement to entire table
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedOutput[i] = outputDataNew;
|
||
}
|
||
outputDataDelayed = outputDataNew;
|
||
}
|
||
|
||
// Reset the stored output quaternion history to current EKF state
|
||
void NavEKF2_core::StoreQuatReset()
|
||
{
|
||
outputDataNew.quat = stateStruct.quat;
|
||
// write current measurement to entire table
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedOutput[i].quat = outputDataNew.quat;
|
||
}
|
||
outputDataDelayed.quat = outputDataNew.quat;
|
||
}
|
||
|
||
// recall output data from the FIFO
|
||
void NavEKF2_core::RecallOutput()
|
||
{
|
||
outputDataDelayed = storedOutput[fifoIndexDelayed];
|
||
}
|
||
|
||
// store imu in the FIFO
|
||
void NavEKF2_core::StoreIMU()
|
||
{
|
||
fifoIndexDelayed = fifoIndexNow;
|
||
fifoIndexNow = fifoIndexNow + 1;
|
||
if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
|
||
fifoIndexNow = 0;
|
||
}
|
||
storedIMU[fifoIndexNow] = imuDataNew;
|
||
}
|
||
|
||
// reset the stored imu history and store the current value
|
||
void NavEKF2_core::StoreIMU_reset()
|
||
{
|
||
// write current measurement to entire table
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedIMU[i] = imuDataNew;
|
||
}
|
||
imuDataDelayed = imuDataNew;
|
||
fifoIndexDelayed = fifoIndexNow+1;
|
||
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
||
fifoIndexDelayed = 0;
|
||
}
|
||
}
|
||
|
||
// recall IMU data from the FIFO
|
||
void NavEKF2_core::RecallIMU()
|
||
{
|
||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||
}
|
||
|
||
// store baro in a history array
|
||
void NavEKF2_core::StoreBaro()
|
||
{
|
||
if (baroStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
baroStoreIndex = 0;
|
||
}
|
||
storedBaro[baroStoreIndex] = baroDataNew;
|
||
baroStoreIndex += 1;
|
||
}
|
||
|
||
// store TAS in a history array
|
||
void NavEKF2_core::StoreTAS()
|
||
{
|
||
if (tasStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
tasStoreIndex = 0;
|
||
}
|
||
storedTAS[tasStoreIndex] = tasDataNew;
|
||
tasStoreIndex += 1;
|
||
}
|
||
|
||
// store OF data in a history array
|
||
void NavEKF2_core::StoreOF()
|
||
{
|
||
if (ofStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
ofStoreIndex = 0;
|
||
}
|
||
storedOF[ofStoreIndex] = ofDataNew;
|
||
ofStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used baro data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallBaro()
|
||
{
|
||
baro_elements dataTemp;
|
||
baro_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedBaro[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// zero the time stamp so we won't use it again
|
||
storedBaro[i]=dataTempZero;
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
baroDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// return newest un-used true airspeed data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallTAS()
|
||
{
|
||
tas_elements dataTemp;
|
||
tas_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedTAS[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// zero the time stamp so we won't use it again
|
||
storedTAS[i]=dataTempZero;
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
tasDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// return newest un-used optical flow data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallOF()
|
||
{
|
||
of_elements dataTemp;
|
||
of_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedOF[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// zero the time stamp so we won't use it again
|
||
storedOF[i]=dataTempZero;
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
ofDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// store magnetometer data in a history array
|
||
void NavEKF2_core::StoreMag()
|
||
{
|
||
if (magStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
magStoreIndex = 0;
|
||
}
|
||
storedMag[magStoreIndex] = magDataNew;
|
||
magStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used magnetometer data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallMag()
|
||
{
|
||
mag_elements dataTemp;
|
||
mag_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedMag[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// zero the time stamp so we won't use it again
|
||
storedMag[i]=dataTempZero;
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
magDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// store GPS data in a history array
|
||
void NavEKF2_core::StoreGPS()
|
||
{
|
||
if (gpsStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
gpsStoreIndex = 0;
|
||
}
|
||
storedGPS[gpsStoreIndex] = gpsDataNew;
|
||
gpsStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used GPS data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallGPS()
|
||
{
|
||
gps_elements dataTemp;
|
||
gps_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedGPS[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// zero the time stamp so we won't use it again
|
||
storedGPS[i]=dataTempZero;
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
gpsDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// calculate nav to body quaternions from body to nav rotation matrix
|
||
void NavEKF2_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
|
||
{
|
||
// Calculate the body to nav cosine matrix
|
||
quat.rotation_matrix(Tbn);
|
||
}
|
||
|
||
// return the Euler roll, pitch and yaw angle in radians
|
||
void NavEKF2_core::getEulerAngles(Vector3f &euler) const
|
||
{
|
||
outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
|
||
euler = euler - _ahrs->get_trim();
|
||
}
|
||
|
||
// This returns the specific forces in the NED frame
|
||
void NavEKF2_core::getAccelNED(Vector3f &accelNED) const {
|
||
accelNED = velDotNED;
|
||
accelNED.z -= GRAVITY_MSS;
|
||
}
|
||
|
||
// return NED velocity in m/s
|
||
//
|
||
void NavEKF2_core::getVelNED(Vector3f &vel) const
|
||
{
|
||
vel = outputDataNew.velocity;
|
||
}
|
||
|
||
// Return the last calculated NED position relative to the reference point (m).
|
||
// if a calculated solution is not available, use the best available data and return false
|
||
bool NavEKF2_core::getPosNED(Vector3f &pos) const
|
||
{
|
||
// The EKF always has a height estimate regardless of mode of operation
|
||
pos.z = outputDataNew.position.z;
|
||
// There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
|
||
nav_filter_status status;
|
||
getFilterStatus(status);
|
||
if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) {
|
||
// This is the normal mode of operation where we can use the EKF position states
|
||
pos.x = outputDataNew.position.x;
|
||
pos.y = outputDataNew.position.y;
|
||
return true;
|
||
} else {
|
||
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
|
||
if(validOrigin) {
|
||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
|
||
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
|
||
pos.x = tempPosNE.x;
|
||
pos.y = tempPosNE.y;
|
||
return false;
|
||
} else {
|
||
// If no GPS fix is available, all we can do is provide the last known position
|
||
pos.x = outputDataNew.position.x + lastKnownPositionNE.x;
|
||
pos.y = outputDataNew.position.y + lastKnownPositionNE.y;
|
||
return false;
|
||
}
|
||
} else {
|
||
// If the origin has not been set, then we have no means of providing a relative position
|
||
pos.x = 0.0f;
|
||
pos.y = 0.0f;
|
||
return false;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// return body axis gyro bias estimates in rad/sec
|
||
void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
|
||
{
|
||
if (dtIMUavg < 1e-6f) {
|
||
gyroBias.zero();
|
||
return;
|
||
}
|
||
gyroBias = stateStruct.gyro_bias / dtIMUavg;
|
||
}
|
||
|
||
// return body axis gyro scale factor estimates
|
||
void NavEKF2_core::getGyroScale(Vector3f &gyroScale) const
|
||
{
|
||
if (!statesInitialised) {
|
||
gyroScale.x = gyroScale.y = gyroScale.z = 0;
|
||
return;
|
||
}
|
||
gyroScale.x = 1.0f/stateStruct.gyro_scale.x - 1.0f;
|
||
gyroScale.y = 1.0f/stateStruct.gyro_scale.y - 1.0f;
|
||
gyroScale.z = 1.0f/stateStruct.gyro_scale.z - 1.0f;
|
||
}
|
||
|
||
// return tilt error convergence metric
|
||
void NavEKF2_core::getTiltError(float &ang) const
|
||
{
|
||
ang = tiltErrFilt;
|
||
}
|
||
|
||
// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
|
||
void NavEKF2_core::resetGyroBias(void)
|
||
{
|
||
stateStruct.gyro_bias.zero();
|
||
zeroRows(P,9,11);
|
||
zeroCols(P,9,11);
|
||
P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg));
|
||
P[10][10] = P[9][9];
|
||
P[11][11] = P[9][9];
|
||
}
|
||
|
||
// Reset the baro so that it reads zero at the current height
|
||
// Reset the EKF height to zero
|
||
// Adjust the EKf origin height so that the EKF height + origin height is the same as before
|
||
// Return true if the height datum reset has been performed
|
||
// If using a range finder for height do not reset and return false
|
||
bool NavEKF2_core::resetHeightDatum(void)
|
||
{
|
||
// if we are using a range finder for height, return false
|
||
if (frontend._altSource == 1) {
|
||
return false;
|
||
}
|
||
// record the old height estimate
|
||
float oldHgt = -stateStruct.position.z;
|
||
// reset the barometer so that it reads zero at the current height
|
||
_baro.update_calibration();
|
||
// reset the height state
|
||
stateStruct.position.z = 0.0f;
|
||
// adjust the height of the EKF origin so that the origin plus baro height before and afer the reset is the same
|
||
if (validOrigin) {
|
||
EKF_origin.alt += oldHgt*100;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
// Commands the EKF to not use GPS.
|
||
// This command must be sent prior to arming
|
||
// This command is forgotten by the EKF each time the vehicle disarms
|
||
// Returns 0 if command rejected
|
||
// Returns 1 if attitude, vertical velocity and vertical position will be provided
|
||
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||
uint8_t NavEKF2_core::setInhibitGPS(void)
|
||
{
|
||
if(!filterArmed) {
|
||
return 0;
|
||
}
|
||
if (optFlowDataPresent()) {
|
||
frontend._fusionModeGPS = 3;
|
||
//#error writing to a tuning parameter
|
||
return 2;
|
||
} else {
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||
void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||
{
|
||
if (PV_AidingMode == AID_RELATIVE) {
|
||
// allow 1.0 rad/sec margin for angular motion
|
||
ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd);
|
||
// use standard gains up to 5.0 metres height and reduce above that
|
||
ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f);
|
||
} else {
|
||
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
|
||
ekfNavVelGainScaler = 1.0f;
|
||
}
|
||
}
|
||
|
||
// return weighting of first IMU in blending function
|
||
// This filter always uses the primary IMU, so a weighting of 1 is returned
|
||
void NavEKF2_core::getIMU1Weighting(float &ret) const
|
||
{
|
||
ret = 1.0f;
|
||
}
|
||
|
||
// return the individual Z-accel bias estimates in m/s^2
|
||
void NavEKF2_core::getAccelZBias(float &zbias1, float &zbias2) const {
|
||
if (dtIMUavg > 0) {
|
||
zbias1 = stateStruct.accel_zbias / dtIMUavg;
|
||
zbias2 = 0;
|
||
} else {
|
||
zbias1 = 0;
|
||
zbias2 = 0;
|
||
}
|
||
}
|
||
|
||
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
|
||
void NavEKF2_core::getWind(Vector3f &wind) const
|
||
{
|
||
wind.x = stateStruct.wind_vel.x;
|
||
wind.y = stateStruct.wind_vel.y;
|
||
wind.z = 0.0f; // currently don't estimate this
|
||
}
|
||
|
||
// return earth magnetic field estimates in measurement units / 1000
|
||
void NavEKF2_core::getMagNED(Vector3f &magNED) const
|
||
{
|
||
magNED = stateStruct.earth_magfield * 1000.0f;
|
||
}
|
||
|
||
// return body magnetic field estimates in measurement units / 1000
|
||
void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
|
||
{
|
||
magXYZ = stateStruct.body_magfield*1000.0f;
|
||
}
|
||
|
||
// return magnetometer offsets
|
||
// return true if offsets are valid
|
||
bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
|
||
{
|
||
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
|
||
if (secondMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
|
||
magOffsets = _ahrs->get_compass()->get_offsets(0) - stateStruct.body_magfield*1000.0f;
|
||
return true;
|
||
} else {
|
||
magOffsets = _ahrs->get_compass()->get_offsets(0);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// Return the last calculated latitude, longitude and height in WGS-84
|
||
// If a calculated location isn't available, return a raw GPS measurement
|
||
// The status will return true if a calculation or raw measurement is available
|
||
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
|
||
bool NavEKF2_core::getLLH(struct Location &loc) const
|
||
{
|
||
if(validOrigin) {
|
||
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
|
||
loc.alt = EKF_origin.alt - outputDataNew.position.z*100;
|
||
loc.flags.relative_alt = 0;
|
||
loc.flags.terrain_alt = 0;
|
||
|
||
// there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding)
|
||
nav_filter_status status;
|
||
getFilterStatus(status);
|
||
if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) {
|
||
loc.lat = EKF_origin.lat;
|
||
loc.lng = EKF_origin.lng;
|
||
location_offset(loc, outputDataNew.position.x, outputDataNew.position.y);
|
||
return true;
|
||
} else {
|
||
// we could be in constant position mode becasue the vehicle has taken off without GPS, or has lost GPS
|
||
// in this mode we cannot use the EKF states to estimate position so will return the best available data
|
||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
|
||
// we have a GPS position fix to return
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
loc.lat = gpsloc.lat;
|
||
loc.lng = gpsloc.lng;
|
||
return true;
|
||
} else {
|
||
// if no GPS fix, provide last known position before entering the mode
|
||
location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
|
||
return false;
|
||
}
|
||
}
|
||
} else {
|
||
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw
|
||
// GPS reading if available and return false
|
||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) {
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
loc = gpsloc;
|
||
loc.flags.relative_alt = 0;
|
||
loc.flags.terrain_alt = 0;
|
||
}
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// return the estimated height above ground level
|
||
bool NavEKF2_core::getHAGL(float &HAGL) const
|
||
{
|
||
HAGL = terrainState - outputDataNew.position.z;
|
||
// If we know the terrain offset and altitude, then we have a valid height above ground estimate
|
||
return !hgtTimeout && gndOffsetValid && healthy();
|
||
}
|
||
|
||
// return data for debugging optical flow fusion
|
||
void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
|
||
{
|
||
varFlow = max(flowTestRatio[0],flowTestRatio[1]);
|
||
gndOffset = terrainState;
|
||
flowInnovX = innovOptFlow[0];
|
||
flowInnovY = innovOptFlow[1];
|
||
auxInnov = auxFlowObsInnov;
|
||
HAGL = terrainState - stateStruct.position.z;
|
||
rngInnov = innovRng;
|
||
range = rngMea;
|
||
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||
}
|
||
|
||
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
|
||
void NavEKF2_core::SetFlightAndFusionModes()
|
||
{
|
||
// determine if the vehicle is manoevring
|
||
if (accNavMagHoriz > 0.5f) {
|
||
manoeuvring = true;
|
||
} else {
|
||
manoeuvring = false;
|
||
}
|
||
// if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
|
||
if (assume_zero_sideslip()) {
|
||
// Evaluate a numerical score that defines the likelihood we are in the air
|
||
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
|
||
bool highGndSpd = false;
|
||
bool highAirSpd = false;
|
||
bool largeHgtChange = false;
|
||
|
||
// trigger at 8 m/s airspeed
|
||
if (_ahrs->airspeed_sensor_enabled()) {
|
||
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
|
||
if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
|
||
highAirSpd = true;
|
||
}
|
||
}
|
||
|
||
// trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
|
||
if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
|
||
highGndSpd = true;
|
||
}
|
||
|
||
// trigger if more than 10m away from initial height
|
||
if (fabsf(baroDataDelayed.hgt) > 10.0f) {
|
||
largeHgtChange = true;
|
||
}
|
||
|
||
// to go to in-air mode we also need enough GPS velocity to be able to calculate a reliable ground track heading and either a lerge height or airspeed change
|
||
if (onGround && highGndSpd && (highAirSpd || largeHgtChange)) {
|
||
onGround = false;
|
||
}
|
||
// if is possible we are in flight, set the time this condition was last detected
|
||
if (highGndSpd || highAirSpd || largeHgtChange) {
|
||
airborneDetectTime_ms = imuSampleTime_ms;
|
||
}
|
||
// after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
|
||
if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
|
||
onGround = true;
|
||
}
|
||
// perform a yaw alignment check against GPS if exiting on-ground mode, bu tonly if we have enough ground speed
|
||
// this is done to protect against unrecoverable heading alignment errors due to compass faults
|
||
if (!onGround && prevOnGround) {
|
||
alignYawGPS();
|
||
}
|
||
}
|
||
// store current on-ground status for next time
|
||
prevOnGround = onGround;
|
||
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
|
||
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || constPosMode);
|
||
// request mag calibration for both in-air and manoeuvre threshold options
|
||
bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3);
|
||
// deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
|
||
bool magCalDenied = !use_compass() || constPosMode || (frontend._magCal == 2);
|
||
// inhibit the magnetic field calibration if not requested or denied
|
||
inhibitMagStates = (!magCalRequested || magCalDenied);
|
||
|
||
if (inhibitMagStates && inhibitWindStates) {
|
||
stateIndexLim = 15;
|
||
} else if (inhibitWindStates) {
|
||
stateIndexLim = 21;
|
||
} else {
|
||
stateIndexLim = 23;
|
||
}
|
||
}
|
||
|
||
// initialise the covariance matrix
|
||
void NavEKF2_core::CovarianceInit()
|
||
{
|
||
// zero the matrix
|
||
for (uint8_t i=1; i<=stateIndexLim; i++)
|
||
{
|
||
for (uint8_t j=0; j<=stateIndexLim; j++)
|
||
{
|
||
P[i][j] = 0.0f;
|
||
}
|
||
}
|
||
// attitude error
|
||
P[0][0] = 0.1f;
|
||
P[1][1] = 0.1f;
|
||
P[2][2] = 0.1f;
|
||
// velocities
|
||
P[3][3] = sq(0.7f);
|
||
P[4][4] = P[3][3];
|
||
P[5][5] = sq(0.7f);
|
||
// positions
|
||
P[6][6] = sq(15.0f);
|
||
P[7][7] = P[6][6];
|
||
P[8][8] = sq(frontend._baroAltNoise);
|
||
// gyro delta angle biases
|
||
P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg));
|
||
P[10][10] = P[9][9];
|
||
P[11][11] = P[9][9];
|
||
// gyro scale factor biases
|
||
P[12][12] = sq(1e-3);
|
||
P[13][13] = P[12][12];
|
||
P[14][14] = P[12][12];
|
||
// Z delta velocity bias
|
||
P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtIMUavg);
|
||
// earth magnetic field
|
||
P[16][16] = 0.0f;
|
||
P[17][17] = P[16][16];
|
||
P[18][18] = P[16][16];
|
||
// body magnetic field
|
||
P[19][19] = 0.0f;
|
||
P[20][20] = P[19][19];
|
||
P[21][21] = P[19][19];
|
||
// wind velocities
|
||
P[22][22] = 0.0f;
|
||
P[23][23] = P[22][22];
|
||
|
||
|
||
// optical flow ground height covariance
|
||
Popt = 0.25f;
|
||
|
||
}
|
||
|
||
// force symmetry on the covariance matrix to prevent ill-conditioning
|
||
void NavEKF2_core::ForceSymmetry()
|
||
{
|
||
for (uint8_t i=1; i<=stateIndexLim; i++)
|
||
{
|
||
for (uint8_t j=0; j<=i-1; j++)
|
||
{
|
||
float temp = 0.5f*(P[i][j] + P[j][i]);
|
||
P[i][j] = temp;
|
||
P[j][i] = temp;
|
||
}
|
||
}
|
||
}
|
||
|
||
// copy covariances across from covariance prediction calculation
|
||
void NavEKF2_core::CopyCovariances()
|
||
{
|
||
// copy predicted covariances
|
||
for (uint8_t i=0; i<=stateIndexLim; i++) {
|
||
for (uint8_t j=0; j<=stateIndexLim; j++)
|
||
{
|
||
P[i][j] = nextP[i][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
// constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
|
||
void NavEKF2_core::ConstrainVariances()
|
||
{
|
||
for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
|
||
for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
|
||
for (uint8_t i=6; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions
|
||
for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMUavg)); // delta angle biases
|
||
for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
|
||
P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtIMUavg)); // delta velocity bias
|
||
for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
|
||
for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field
|
||
for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity
|
||
}
|
||
|
||
// constrain states to prevent ill-conditioning
|
||
void NavEKF2_core::ConstrainStates()
|
||
{
|
||
// attitude errors are limited between +-1
|
||
for (uint8_t i=0; i<=2; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
|
||
// velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
|
||
for (uint8_t i=3; i<=5; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f);
|
||
// position limit 1000 km - TODO apply circular limit
|
||
for (uint8_t i=6; i<=7; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f);
|
||
// height limit covers home alt on everest through to home alt at SL and ballon drop
|
||
stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
|
||
// gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs)
|
||
for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-0.1f*dtIMUavg,0.1f*dtIMUavg);
|
||
// gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs)
|
||
for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f);
|
||
// Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
|
||
stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtIMUavg,1.0f*dtIMUavg);
|
||
// earth magnetic field limit
|
||
for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
|
||
// body magnetic field limit
|
||
for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f);
|
||
// wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
|
||
for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f);
|
||
// constrain the terrain offset state
|
||
terrainState = max(terrainState, stateStruct.position.z + rngOnGnd);
|
||
}
|
||
|
||
bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
|
||
if (ins_index < ins.get_accel_count()) {
|
||
ins.get_delta_velocity(ins_index,dVel);
|
||
dVel_dt = max(ins.get_delta_velocity_dt(ins_index),1.0e-4f);
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
|
||
if (ins_index < ins.get_gyro_count()) {
|
||
ins.get_delta_angle(ins_index,dAng);
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// update IMU delta angle and delta velocity measurements
|
||
void NavEKF2_core::readIMUData()
|
||
{
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
|
||
// average IMU sampling rate
|
||
dtIMUavg = 1.0f/ins.get_sample_rate();
|
||
|
||
// the imu sample time is used as a common time reference throughout the filter
|
||
imuSampleTime_ms = hal.scheduler->millis();
|
||
|
||
// Get delta velocity data
|
||
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
|
||
|
||
// Get delta angle data
|
||
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
|
||
imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f);
|
||
|
||
// get current time stamp
|
||
imuDataNew.time_ms = imuSampleTime_ms;
|
||
|
||
// save data in the FIFO buffer
|
||
StoreIMU();
|
||
|
||
// extract the oldest available data from the FIFO buffer
|
||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||
|
||
}
|
||
|
||
// check for new valid GPS data and update stored measurement if available
|
||
void NavEKF2_core::readGpsData()
|
||
{
|
||
// check for new GPS data
|
||
if ((_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) &&
|
||
(_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D))
|
||
{
|
||
// store fix time from previous read
|
||
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
|
||
|
||
// get current fix time
|
||
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
|
||
|
||
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
|
||
// ideally we should be using a timing signal from the GPS receiver to set this time
|
||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
|
||
|
||
// read the NED velocity from the GPS
|
||
gpsDataNew.vel = _ahrs->get_gps().velocity();
|
||
|
||
// Use the speed accuracy from the GPS if available, otherwise set it to zero.
|
||
// Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
|
||
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
|
||
gpsSpdAccuracy *= (1.0f - alpha);
|
||
float gpsSpdAccRaw;
|
||
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
|
||
gpsSpdAccuracy = 0.0f;
|
||
} else {
|
||
gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
|
||
}
|
||
|
||
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
|
||
if (_ahrs->get_gps().num_sats() >= 6 && !constPosMode) {
|
||
gpsNoiseScaler = 1.0f;
|
||
} else if (_ahrs->get_gps().num_sats() == 5 && !constPosMode) {
|
||
gpsNoiseScaler = 1.4f;
|
||
} else { // <= 4 satellites or in constant position mode
|
||
gpsNoiseScaler = 2.0f;
|
||
}
|
||
|
||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
||
if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
|
||
useGpsVertVel = true;
|
||
} else {
|
||
useGpsVertVel = false;
|
||
}
|
||
|
||
// Monitor quality of the GPS velocity data for alignment
|
||
gpsQualGood = calcGpsGoodToAlign();
|
||
|
||
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
|
||
// If we don't have an origin, then set it to the current GPS coordinates
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
if (validOrigin) {
|
||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||
} else if (gpsQualGood) {
|
||
// Set the NE origin to the current GPS position
|
||
setOrigin();
|
||
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
|
||
alignMagStateDeclination();
|
||
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
|
||
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
|
||
// We are by definition at the origin at the instant of alignment so set NE position to zero
|
||
gpsDataNew.pos.zero();
|
||
// If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode
|
||
if (filterArmed && frontend._fusionModeGPS != 3) {
|
||
constPosMode = false;
|
||
PV_AidingMode = AID_ABSOLUTE;
|
||
gpsNotAvailable = false;
|
||
// Initialise EKF position and velocity states
|
||
ResetPosition();
|
||
ResetVelocity();
|
||
}
|
||
}
|
||
|
||
// calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
|
||
decayGpsOffset();
|
||
|
||
// save measurement to buffer to be fused later
|
||
StoreGPS();
|
||
|
||
// declare GPS available for use
|
||
gpsNotAvailable = false;
|
||
}
|
||
|
||
// If not aiding we synthesise the GPS measurements at a zero position
|
||
if (PV_AidingMode == AID_NONE) {
|
||
gpsNotAvailable = true;
|
||
if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
|
||
gpsDataNew.pos.zero();
|
||
gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
|
||
// save measurement to buffer to be fused later
|
||
StoreGPS();
|
||
}
|
||
} else if (PV_AidingMode == AID_RELATIVE) {
|
||
gpsNotAvailable = true;
|
||
}
|
||
|
||
}
|
||
|
||
// check for new altitude measurement data and update stored measurement if available
|
||
void NavEKF2_core::readHgtData()
|
||
{
|
||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||
if (_baro.get_last_update() != lastHgtReceived_ms) {
|
||
// Don't use Baro height if operating in optical flow mode as we use range finder instead
|
||
if (frontend._fusionModeGPS == 3 && frontend._altSource == 1) {
|
||
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
|
||
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor
|
||
baroDataNew.hgt = max(rngMea * Tnb_flow.c.z, rngOnGnd);
|
||
// calculate offset to baro data that enables baro to be used as a backup
|
||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||
} else if (filterArmed && takeOffDetected) {
|
||
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
|
||
baroDataNew.hgt = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
|
||
} else {
|
||
// If we are on ground and have no range finder reading, assume the nominal on-ground height
|
||
baroDataNew.hgt = rngOnGnd;
|
||
// calculate offset to baro data that enables baro to be used as a backup
|
||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||
}
|
||
} else {
|
||
// use baro measurement and correct for baro offset
|
||
baroDataNew.hgt = _baro.get_altitude();
|
||
}
|
||
|
||
// filtered baro data used to provide a reference for takeoff
|
||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||
if (!getTakeoffExpected()) {
|
||
const float gndHgtFiltTC = 0.5f;
|
||
const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
|
||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||
} else if (filterArmed && getTakeoffExpected()) {
|
||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
||
baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff);
|
||
}
|
||
|
||
// time stamp used to check for new measurement
|
||
lastHgtReceived_ms = _baro.get_last_update();
|
||
|
||
// estimate of time height measurement was taken, allowing for delays
|
||
hgtMeasTime_ms = lastHgtReceived_ms - frontend._hgtDelay_ms;
|
||
|
||
// save baro measurement to buffer to be fused later
|
||
baroDataNew.time_ms = hgtMeasTime_ms;
|
||
StoreBaro();
|
||
}
|
||
}
|
||
|
||
// check for new magnetometer data and update store measurements if available
|
||
void NavEKF2_core::readMagData()
|
||
{
|
||
if (use_compass() && _ahrs->get_compass()->last_update_usec() != lastMagUpdate_ms) {
|
||
// store time of last measurement update
|
||
lastMagUpdate_ms = _ahrs->get_compass()->last_update_usec();
|
||
|
||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||
magMeasTime_ms = imuSampleTime_ms - frontend.magDelay_ms;
|
||
|
||
// read compass data and scale to improve numerical conditioning
|
||
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
|
||
|
||
// check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations
|
||
if (_ahrs->get_compass()->healthy(0)) {
|
||
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
|
||
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
|
||
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
|
||
if (changeDetected && secondMagYawInit) {
|
||
stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
|
||
stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
|
||
stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
|
||
}
|
||
lastMagOffsets = nowMagOffsets;
|
||
}
|
||
|
||
// save magnetometer measurement to buffer to be fused later
|
||
magDataNew.time_ms = magMeasTime_ms;
|
||
StoreMag();
|
||
}
|
||
}
|
||
|
||
// check for new airspeed data and update stored measurements if available
|
||
void NavEKF2_core::readAirSpdData()
|
||
{
|
||
// if airspeed reading is valid and is set by the user to be used and has been updated then
|
||
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
|
||
// know a new measurement is available
|
||
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
|
||
if (aspeed &&
|
||
aspeed->use() &&
|
||
aspeed->last_update_ms() != timeTasReceived_ms) {
|
||
tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
|
||
timeTasReceived_ms = aspeed->last_update_ms();
|
||
tasDataNew.time_ms = timeTasReceived_ms - frontend.tasDelay_ms;
|
||
newDataTas = true;
|
||
StoreTAS();
|
||
RecallTAS();
|
||
} else {
|
||
newDataTas = false;
|
||
}
|
||
}
|
||
|
||
// write the raw optical flow measurements
|
||
// this needs to be called externally.
|
||
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
|
||
{
|
||
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
|
||
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
|
||
// A positive X rate is produced by a positive sensor rotation about the X axis
|
||
// A positive Y rate is produced by a positive sensor rotation about the Y axis
|
||
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
|
||
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
|
||
flowMeaTime_ms = imuSampleTime_ms;
|
||
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
|
||
// reset the accumulated body delta angle and time
|
||
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
|
||
if (delTimeOF > 0.01f) {
|
||
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
|
||
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
|
||
delAngBodyOF.zero();
|
||
delTimeOF = 0.0f;
|
||
}
|
||
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
|
||
// if we haven't taken off - constrain position and velocity states
|
||
if (frontend._fusionModeGPS == 3) {
|
||
detectOptFlowTakeoff();
|
||
}
|
||
// calculate rotation matrices at mid sample time for flow observations
|
||
stateStruct.quat.rotation_matrix(Tbn_flow);
|
||
Tnb_flow = Tbn_flow.transposed();
|
||
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
|
||
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
|
||
// correct flow sensor rates for bias
|
||
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
|
||
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
|
||
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
|
||
// note correction for different axis and sign conventions used by the px4flow sensor
|
||
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
|
||
// write flow rate measurements corrected for body rates
|
||
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
|
||
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
|
||
// record time last observation was received so we can detect loss of data elsewhere
|
||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||
// estimate sample time of the measurement
|
||
ofDataNew.time_ms = imuSampleTime_ms - frontend._flowDelay_ms - frontend.flowTimeDeltaAvg_ms/2;
|
||
// Save data to buffer
|
||
StoreOF();
|
||
// Check for data at the fusion time horizon
|
||
newDataFlow = RecallOF();
|
||
}
|
||
}
|
||
|
||
// calculate the NED earth spin vector in rad/sec
|
||
void NavEKF2_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
|
||
{
|
||
float lat_rad = radians(latitude*1.0e-7f);
|
||
omega.x = earthRate*cosf(lat_rad);
|
||
omega.y = 0;
|
||
omega.z = -earthRate*sinf(lat_rad);
|
||
}
|
||
|
||
// initialise the earth magnetic field states using declination, suppled roll/pitch
|
||
// and magnetometer measurements and return initial attitude quaternion
|
||
// if no magnetometer data, do not update magnetic field states and assume zero yaw angle
|
||
Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
|
||
{
|
||
// declare local variables required to calculate initial orientation and magnetic field
|
||
float yaw;
|
||
Matrix3f Tbn;
|
||
Vector3f initMagNED;
|
||
Quaternion initQuat;
|
||
|
||
if (use_compass()) {
|
||
// calculate rotation matrix from body to NED frame
|
||
Tbn.from_euler(roll, pitch, 0.0f);
|
||
|
||
// read the magnetometer data
|
||
readMagData();
|
||
|
||
// rotate the magnetic field into NED axes
|
||
initMagNED = Tbn * magDataDelayed.mag;
|
||
|
||
// calculate heading of mag field rel to body heading
|
||
float magHeading = atan2f(initMagNED.y, initMagNED.x);
|
||
|
||
// get the magnetic declination
|
||
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
|
||
|
||
// calculate yaw angle rel to true north
|
||
yaw = magDecAng - magHeading;
|
||
yawAligned = true;
|
||
// calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
|
||
// otherwise use existing heading
|
||
if (!badMag) {
|
||
// store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
|
||
Vector3f tempEuler;
|
||
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
|
||
yawResetAngle = wrap_PI(yaw - tempEuler.z);
|
||
// set the flag to indicate that an in-flight yaw reset has been performed
|
||
// this will be cleared when the reset value is retrieved
|
||
yawResetAngleWaiting = true;
|
||
// calculate an initial quaternion using the new yaw value
|
||
initQuat.from_euler(roll, pitch, yaw);
|
||
} else {
|
||
initQuat = stateStruct.quat;
|
||
}
|
||
|
||
// calculate initial Tbn matrix and rotate Mag measurements into NED
|
||
// to set initial NED magnetic field states
|
||
initQuat.rotation_matrix(Tbn);
|
||
stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
|
||
|
||
// align the NE earth magnetic field states with the published declination
|
||
alignMagStateDeclination();
|
||
|
||
// clear bad magnetometer status
|
||
badMag = false;
|
||
} else {
|
||
initQuat.from_euler(roll, pitch, 0.0f);
|
||
yawAligned = false;
|
||
}
|
||
|
||
// return attitude quaternion
|
||
return initQuat;
|
||
}
|
||
|
||
// this function is used to do a forced alignment of the yaw angle to align with the horizontal velocity
|
||
// vector from GPS. It is used to align the yaw angle after launch or takeoff.
|
||
void NavEKF2_core::alignYawGPS()
|
||
{
|
||
if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
|
||
float roll;
|
||
float pitch;
|
||
float oldYaw;
|
||
float newYaw;
|
||
float yawErr;
|
||
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
|
||
stateStruct.quat.to_euler(roll, pitch, oldYaw);
|
||
// calculate course yaw angle
|
||
oldYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x);
|
||
// calculate yaw angle from GPS velocity
|
||
newYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
|
||
// estimate the yaw error
|
||
yawErr = wrap_PI(newYaw - oldYaw);
|
||
// If the inertial course angle disagrees with the GPS by more than 45 degrees, we declare the compass as bad
|
||
badMag = (fabsf(yawErr) > 0.7854f);
|
||
// correct yaw angle using GPS ground course compass failed or if not previously aligned
|
||
if (badMag || !yawAligned) {
|
||
// correct the yaw angle
|
||
newYaw = oldYaw + yawErr;
|
||
// calculate new filter quaternion states from Euler angles
|
||
stateStruct.quat.from_euler(roll, pitch, newYaw);
|
||
// the yaw angle is now aligned so update its status
|
||
yawAligned = true;
|
||
// reset the position and velocity states
|
||
ResetPosition();
|
||
ResetVelocity();
|
||
// reset the covariance for the quaternion, velocity and position states
|
||
// zero the matrix entries
|
||
zeroRows(P,0,9);
|
||
zeroCols(P,0,9);
|
||
// velocities - we could have a big error coming out of constant position mode due to GPS lag
|
||
P[3][3] = 400.0f;
|
||
P[4][4] = P[3][3];
|
||
P[5][5] = sq(0.7f);
|
||
// positions - we could have a big error coming out of constant position mode due to GPS lag
|
||
P[6][6] = 400.0f;
|
||
P[7][7] = P[6][6];
|
||
P[8][8] = sq(5.0f);
|
||
}
|
||
// Update magnetic field states if the magnetometer is bad
|
||
if (badMag) {
|
||
Vector3f eulerAngles;
|
||
getEulerAngles(eulerAngles);
|
||
calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||
}
|
||
}
|
||
}
|
||
|
||
// return the transformation matrix from XYZ (body) to NED axes
|
||
void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
|
||
{
|
||
Vector3f trim = _ahrs->get_trim();
|
||
outputDataNew.quat.rotation_matrix(mat);
|
||
mat.rotateXYinv(trim);
|
||
}
|
||
|
||
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
|
||
void NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
|
||
{
|
||
velInnov.x = innovVelPos[0];
|
||
velInnov.y = innovVelPos[1];
|
||
velInnov.z = innovVelPos[2];
|
||
posInnov.x = innovVelPos[3];
|
||
posInnov.y = innovVelPos[4];
|
||
posInnov.z = innovVelPos[5];
|
||
magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units
|
||
magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units
|
||
magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units
|
||
tasInnov = innovVtas;
|
||
yawInnov = innovYaw;
|
||
}
|
||
|
||
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
|
||
// this indicates the amount of margin available when tuning the various error traps
|
||
// also return the current offsets applied to the GPS position measurements
|
||
void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
|
||
{
|
||
velVar = sqrtf(velTestRatio);
|
||
posVar = sqrtf(posTestRatio);
|
||
hgtVar = sqrtf(hgtTestRatio);
|
||
magVar.x = sqrtf(magTestRatio.x);
|
||
magVar.y = sqrtf(magTestRatio.y);
|
||
magVar.z = sqrtf(magTestRatio.z);
|
||
tasVar = sqrtf(tasTestRatio);
|
||
offset = gpsPosGlitchOffsetNE;
|
||
}
|
||
|
||
// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
|
||
void NavEKF2_core::InitialiseVariables()
|
||
{
|
||
// initialise time stamps
|
||
imuSampleTime_ms = hal.scheduler->millis();
|
||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||
prevTasStep_ms = imuSampleTime_ms;
|
||
prevBetaStep_ms = imuSampleTime_ms;
|
||
lastMagUpdate_ms = 0;
|
||
lastHgtReceived_ms = imuSampleTime_ms;
|
||
lastVelPassTime_ms = imuSampleTime_ms;
|
||
lastPosPassTime_ms = imuSampleTime_ms;
|
||
lastPosFailTime_ms = 0;
|
||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||
lastTasPassTime_ms = imuSampleTime_ms;
|
||
lastTimeGpsReceived_ms = 0;
|
||
secondLastGpsTime_ms = 0;
|
||
lastDecayTime_ms = imuSampleTime_ms;
|
||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
flowMeaTime_ms = 0;
|
||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||
gndHgtValidTime_ms = 0;
|
||
ekfStartTime_ms = imuSampleTime_ms;
|
||
lastGpsVelFail_ms = 0;
|
||
lastGpsAidBadTime_ms = 0;
|
||
hgtMeasTime_ms = imuSampleTime_ms;
|
||
magMeasTime_ms = imuSampleTime_ms;
|
||
timeTasReceived_ms = 0;
|
||
|
||
// initialise other variables
|
||
gpsNoiseScaler = 1.0f;
|
||
hgtTimeout = true;
|
||
magTimeout = true;
|
||
tasTimeout = true;
|
||
badMag = false;
|
||
badIMUdata = false;
|
||
firstArmComplete = false;
|
||
firstMagYawInit = false;
|
||
secondMagYawInit = false;
|
||
dtIMUavg = 0.0025f;
|
||
dt = 0;
|
||
velDotNEDfilt.zero();
|
||
summedDelAng.zero();
|
||
summedDelVel.zero();
|
||
gpsPosGlitchOffsetNE.zero();
|
||
lastKnownPositionNE.zero();
|
||
prevTnb.zero();
|
||
memset(&P[0][0], 0, sizeof(P));
|
||
memset(&nextP[0][0], 0, sizeof(nextP));
|
||
memset(&processNoise[0], 0, sizeof(processNoise));
|
||
flowDataValid = false;
|
||
newDataRng = false;
|
||
fuseOptFlowData = false;
|
||
Popt = 0.0f;
|
||
terrainState = 0.0f;
|
||
prevPosN = stateStruct.position.x;
|
||
prevPosE = stateStruct.position.y;
|
||
fuseRngData = false;
|
||
inhibitGndState = true;
|
||
flowGyroBias.x = 0;
|
||
flowGyroBias.y = 0;
|
||
constVelMode = false;
|
||
lastConstVelMode = false;
|
||
heldVelNE.zero();
|
||
PV_AidingMode = AID_NONE;
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
gpsVelGlitchOffset.zero();
|
||
filterArmed = false;
|
||
prevFilterArmed = false;
|
||
constPosMode = true;
|
||
memset(&faultStatus, 0, sizeof(faultStatus));
|
||
hgtRate = 0.0f;
|
||
mag_state.q0 = 1;
|
||
mag_state.DCM.identity();
|
||
onGround = true;
|
||
manoeuvring = false;
|
||
yawAligned = false;
|
||
inhibitWindStates = true;
|
||
inhibitMagStates = true;
|
||
gndOffsetValid = false;
|
||
validOrigin = false;
|
||
takeoffExpectedSet_ms = 0;
|
||
expectGndEffectTakeoff = false;
|
||
touchdownExpectedSet_ms = 0;
|
||
expectGndEffectTouchdown = false;
|
||
gpsSpdAccuracy = 0.0f;
|
||
baroHgtOffset = 0.0f;
|
||
gpsAidingBad = false;
|
||
highYawRate = false;
|
||
yawRateFilt = 0.0f;
|
||
yawResetAngle = 0.0f;
|
||
yawResetAngleWaiting = false;
|
||
tiltErrFilt = 1.0f;
|
||
tiltAlignComplete = false;
|
||
yawAlignComplete = false;
|
||
stateIndexLim = 23;
|
||
imuDataNew.frame = 0;
|
||
baroStoreIndex = 0;
|
||
magStoreIndex = 0;
|
||
gpsStoreIndex = 0;
|
||
tasStoreIndex = 0;
|
||
ofStoreIndex = 0;
|
||
delAngCorrection.zero();
|
||
delVelCorrection.zero();
|
||
velCorrection.zero();
|
||
gpsQualGood = false;
|
||
|
||
}
|
||
|
||
// return true if we should use the airspeed sensor
|
||
bool NavEKF2_core::useAirspeed(void) const
|
||
{
|
||
return _ahrs->airspeed_sensor_enabled();
|
||
}
|
||
|
||
// return true if we should use the range finder sensor
|
||
bool NavEKF2_core::useRngFinder(void) const
|
||
{
|
||
// TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
|
||
return true;
|
||
}
|
||
|
||
// return true if optical flow data is available
|
||
bool NavEKF2_core::optFlowDataPresent(void) const
|
||
{
|
||
return (imuSampleTime_ms - flowMeaTime_ms < 200);
|
||
}
|
||
|
||
// return true if the filter to be ready to use gps
|
||
bool NavEKF2_core::readyToUseGPS(void) const
|
||
{
|
||
return validOrigin && tiltAlignComplete && yawAlignComplete;
|
||
}
|
||
|
||
// return true if we should use the compass
|
||
bool NavEKF2_core::use_compass(void) const
|
||
{
|
||
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
|
||
}
|
||
|
||
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
|
||
// limit radius to a maximum of 50m
|
||
void NavEKF2_core::decayGpsOffset()
|
||
{
|
||
float offsetDecaySpd;
|
||
if (assume_zero_sideslip()) {
|
||
offsetDecaySpd = 5.0f;
|
||
} else {
|
||
offsetDecaySpd = 1.0f;
|
||
}
|
||
float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
|
||
lastDecayTime_ms = imuSampleTime_ms;
|
||
float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
|
||
// decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
|
||
if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
|
||
// Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
|
||
gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
|
||
// calculate scale factor to be applied to both offset components
|
||
float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
|
||
gpsPosGlitchOffsetNE.x *= scaleFactor;
|
||
gpsPosGlitchOffsetNE.y *= scaleFactor;
|
||
} else {
|
||
gpsVelGlitchOffset.zero();
|
||
gpsPosGlitchOffsetNE.zero();
|
||
}
|
||
}
|
||
|
||
/*
|
||
should we assume zero sideslip?
|
||
*/
|
||
bool NavEKF2_core::assume_zero_sideslip(void) const
|
||
{
|
||
// we don't assume zero sideslip for ground vehicles as EKF could
|
||
// be quite sensitive to a rapid spin of the ground vehicle if
|
||
// traction is lost
|
||
return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND;
|
||
}
|
||
|
||
|
||
/*
|
||
vehicle specific initial gyro bias uncertainty in deg/sec
|
||
*/
|
||
float NavEKF2_core::InitialGyroBiasUncertainty(void) const
|
||
{
|
||
return 5.0f;
|
||
}
|
||
|
||
/*
|
||
return the filter fault status as a bitmasked integer
|
||
0 = quaternions are NaN
|
||
1 = velocities are NaN
|
||
2 = badly conditioned X magnetometer fusion
|
||
3 = badly conditioned Y magnetometer fusion
|
||
5 = badly conditioned Z magnetometer fusion
|
||
6 = badly conditioned airspeed fusion
|
||
7 = badly conditioned synthetic sideslip fusion
|
||
7 = filter is not initialised
|
||
*/
|
||
void NavEKF2_core::getFilterFaults(uint8_t &faults) const
|
||
{
|
||
faults = (stateStruct.quat.is_nan()<<0 |
|
||
stateStruct.velocity.is_nan()<<1 |
|
||
faultStatus.bad_xmag<<2 |
|
||
faultStatus.bad_ymag<<3 |
|
||
faultStatus.bad_zmag<<4 |
|
||
faultStatus.bad_airspeed<<5 |
|
||
faultStatus.bad_sideslip<<6 |
|
||
!statesInitialised<<7);
|
||
}
|
||
|
||
/*
|
||
return filter timeout status as a bitmasked integer
|
||
0 = position measurement timeout
|
||
1 = velocity measurement timeout
|
||
2 = height measurement timeout
|
||
3 = magnetometer measurement timeout
|
||
4 = true airspeed measurement timeout
|
||
5 = unassigned
|
||
6 = unassigned
|
||
7 = unassigned
|
||
*/
|
||
void NavEKF2_core::getFilterTimeouts(uint8_t &timeouts) const
|
||
{
|
||
timeouts = (posTimeout<<0 |
|
||
velTimeout<<1 |
|
||
hgtTimeout<<2 |
|
||
magTimeout<<3 |
|
||
tasTimeout<<4);
|
||
}
|
||
|
||
/*
|
||
return filter function status as a bitmasked integer
|
||
0 = attitude estimate valid
|
||
1 = horizontal velocity estimate valid
|
||
2 = vertical velocity estimate valid
|
||
3 = relative horizontal position estimate valid
|
||
4 = absolute horizontal position estimate valid
|
||
5 = vertical position estimate valid
|
||
6 = terrain height estimate valid
|
||
7 = constant position mode
|
||
*/
|
||
void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
|
||
{
|
||
// init return value
|
||
status.value = 0;
|
||
|
||
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
|
||
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
|
||
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
|
||
bool notDeadReckoning = !constVelMode && !constPosMode;
|
||
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
|
||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
|
||
bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3);
|
||
bool gpsNavPossible = !gpsNotAvailable && (frontend._fusionModeGPS <= 2);
|
||
bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete;
|
||
|
||
// set individual flags
|
||
status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
|
||
status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid
|
||
status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
|
||
status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid
|
||
status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid
|
||
status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid
|
||
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
|
||
status.flags.const_pos_mode = constPosMode && filterHealthy; // constant position mode
|
||
status.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy; // we should be able to estimate a relative position when we enter flight mode
|
||
status.flags.pred_horiz_pos_abs = gpsNavPossible && filterHealthy; // we should be able to estimate an absolute position when we enter flight mode
|
||
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
|
||
status.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
|
||
status.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
|
||
status.flags.using_gps = (imuSampleTime_ms - lastPosPassTime_ms) < 4000;
|
||
}
|
||
|
||
// send an EKF_STATUS message to GCS
|
||
void NavEKF2_core::send_status_report(mavlink_channel_t chan)
|
||
{
|
||
// get filter status
|
||
nav_filter_status filt_state;
|
||
getFilterStatus(filt_state);
|
||
|
||
// prepare flags
|
||
uint16_t flags = 0;
|
||
if (filt_state.flags.attitude) {
|
||
flags |= EKF_ATTITUDE;
|
||
}
|
||
if (filt_state.flags.horiz_vel) {
|
||
flags |= EKF_VELOCITY_HORIZ;
|
||
}
|
||
if (filt_state.flags.vert_vel) {
|
||
flags |= EKF_VELOCITY_VERT;
|
||
}
|
||
if (filt_state.flags.horiz_pos_rel) {
|
||
flags |= EKF_POS_HORIZ_REL;
|
||
}
|
||
if (filt_state.flags.horiz_pos_abs) {
|
||
flags |= EKF_POS_HORIZ_ABS;
|
||
}
|
||
if (filt_state.flags.vert_pos) {
|
||
flags |= EKF_POS_VERT_ABS;
|
||
}
|
||
if (filt_state.flags.terrain_alt) {
|
||
flags |= EKF_POS_VERT_AGL;
|
||
}
|
||
if (filt_state.flags.const_pos_mode) {
|
||
flags |= EKF_CONST_POS_MODE;
|
||
}
|
||
if (filt_state.flags.pred_horiz_pos_rel) {
|
||
flags |= EKF_PRED_POS_HORIZ_REL;
|
||
}
|
||
if (filt_state.flags.pred_horiz_pos_abs) {
|
||
flags |= EKF_PRED_POS_HORIZ_ABS;
|
||
}
|
||
|
||
// get variances
|
||
float velVar, posVar, hgtVar, tasVar;
|
||
Vector3f magVar;
|
||
Vector2f offset;
|
||
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||
|
||
// send message
|
||
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), tasVar);
|
||
|
||
}
|
||
|
||
// Check arm status and perform required checks and mode changes
|
||
void NavEKF2_core::performArmingChecks()
|
||
{
|
||
// don't allow filter to arm until it has been running for long enough to stabilise
|
||
prevFilterArmed = filterArmed;
|
||
filterArmed = ((readyToUseGPS() || frontend._fusionModeGPS == 3) && (imuSampleTime_ms - ekfStartTime_ms) > 1000);
|
||
|
||
// check to see if arm status has changed and reset states if it has
|
||
if (filterArmed != prevFilterArmed) {
|
||
// only reset the magnetic field and heading on the first arm. This prevents in-flight learning being forgotten for vehicles that do multiple short flights and disarm in-between.
|
||
if (filterArmed && !firstArmComplete) {
|
||
firstArmComplete = true;
|
||
Vector3f eulerAngles;
|
||
getEulerAngles(eulerAngles);
|
||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||
StoreQuatReset();
|
||
}
|
||
// store vertical position at arming to use as a reference for ground relative cehcks
|
||
if (filterArmed) {
|
||
posDownAtArming = stateStruct.position.z;
|
||
}
|
||
// zero stored velocities used to do dead-reckoning
|
||
heldVelNE.zero();
|
||
// reset the flag that indicates takeoff for use by optical flow navigation
|
||
takeOffDetected = false;
|
||
// set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
|
||
if (!filterArmed) {
|
||
PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
constPosMode = true;
|
||
constVelMode = false; // always clear constant velocity mode if constant position mode is active
|
||
lastConstVelMode = false;
|
||
// store the current position to be used to keep reporting the last known position when disarmed
|
||
lastKnownPositionNE.x = stateStruct.position.x;
|
||
lastKnownPositionNE.y = stateStruct.position.y;
|
||
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
|
||
// this reduces the time required for the filter to settle before the estimate can be used
|
||
meaHgtAtTakeOff = baroDataDelayed.hgt;
|
||
// reset the vertical position state to faster recover from baro errors experienced during touchdown
|
||
stateStruct.position.z = -meaHgtAtTakeOff;
|
||
} else if (frontend._fusionModeGPS == 3) { // arming when GPS useage has been prohibited
|
||
if (optFlowDataPresent()) {
|
||
hal.console->printf("EKF is using optical flow\n");
|
||
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
constPosMode = false;
|
||
constVelMode = false;
|
||
} else {
|
||
hal.console->printf("EKF cannot use aiding\n");
|
||
PV_AidingMode = AID_NONE; // we don't have optical flow data and will only be able to estimate orientation and height
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
constPosMode = true;
|
||
constVelMode = false; // always clear constant velocity mode if constant position mode is active
|
||
}
|
||
// Reset the last valid flow measurement time
|
||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||
// Reset the last valid flow fusion time
|
||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||
// this avoids issues casued by the time delay associated with arming that can trigger short timeouts
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
// store the range finder measurement which will be used as a reference to detect when we have taken off
|
||
rangeAtArming = rngMea;
|
||
// set the time at which we arm to assist with takeoff detection
|
||
timeAtArming_ms = imuSampleTime_ms;
|
||
} else { // arming when GPS useage is allowed
|
||
if (!gpsQualGood) {
|
||
hal.console->printf("EKF cannot use aiding\n");
|
||
PV_AidingMode = AID_NONE; // we don't have have GPS data and will only be able to estimate orientation and height
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
constPosMode = true;
|
||
constVelMode = false; // always clear constant velocity mode if constant position mode is active
|
||
} else {
|
||
hal.console->printf("EKF is using GPS\n");
|
||
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
|
||
posTimeout = false;
|
||
velTimeout = false;
|
||
constPosMode = false;
|
||
constVelMode = false;
|
||
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
|
||
// this is becasue the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
|
||
lastTimeGpsReceived_ms = imuSampleTime_ms;
|
||
secondLastGpsTime_ms = imuSampleTime_ms;
|
||
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
|
||
lastPosPassTime_ms = imuSampleTime_ms;
|
||
// reset the fail time to prevent premature reporting of loss of position accruacy
|
||
lastPosFailTime_ms = 0;
|
||
}
|
||
}
|
||
// Reset all position, velocity and covariance
|
||
ResetVelocity();
|
||
ResetPosition();
|
||
CovarianceInit();
|
||
|
||
} else if (filterArmed && !firstMagYawInit && (stateStruct.position.z - posDownAtArming) < -1.5f && !assume_zero_sideslip()) {
|
||
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
|
||
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
|
||
Vector3f eulerAngles;
|
||
getEulerAngles(eulerAngles);
|
||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||
StoreQuatReset();
|
||
firstMagYawInit = true;
|
||
} else if (filterArmed && !secondMagYawInit && (stateStruct.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip()) {
|
||
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
|
||
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
|
||
Vector3f eulerAngles;
|
||
getEulerAngles(eulerAngles);
|
||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||
StoreQuatReset();
|
||
secondMagYawInit = true;
|
||
}
|
||
|
||
// Always turn aiding off when the vehicle is disarmed
|
||
if (!filterArmed) {
|
||
PV_AidingMode = AID_NONE;
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
// set constant position mode if aiding is inhibited
|
||
constPosMode = true;
|
||
constVelMode = false; // always clear constant velocity mode if constant position mode is active
|
||
lastConstVelMode = false;
|
||
}
|
||
|
||
}
|
||
|
||
// Set the NED origin to be used until the next filter reset
|
||
void NavEKF2_core::setOrigin()
|
||
{
|
||
// assume origin at current GPS location (no averaging)
|
||
EKF_origin = _ahrs->get_gps().location();
|
||
// define Earth rotation vector in the NED navigation frame at the origin
|
||
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
|
||
validOrigin = true;
|
||
hal.console->printf("EKF Origin Set\n");
|
||
}
|
||
|
||
// return the LLH location of the filters NED origin
|
||
bool NavEKF2_core::getOriginLLH(struct Location &loc) const
|
||
{
|
||
if (validOrigin) {
|
||
loc = EKF_origin;
|
||
}
|
||
return validOrigin;
|
||
}
|
||
|
||
// set the LLH location of the filters NED origin
|
||
bool NavEKF2_core::setOriginLLH(struct Location &loc)
|
||
{
|
||
if (filterArmed) {
|
||
return false;
|
||
}
|
||
EKF_origin = loc;
|
||
validOrigin = true;
|
||
return true;
|
||
}
|
||
|
||
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
|
||
bool NavEKF2_core::getTakeoffExpected()
|
||
{
|
||
if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend.gndEffectTimeout_ms) {
|
||
expectGndEffectTakeoff = false;
|
||
}
|
||
|
||
return expectGndEffectTakeoff;
|
||
}
|
||
|
||
// called by vehicle code to specify that a takeoff is happening
|
||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||
void NavEKF2_core::setTakeoffExpected(bool val)
|
||
{
|
||
takeoffExpectedSet_ms = imuSampleTime_ms;
|
||
expectGndEffectTakeoff = val;
|
||
}
|
||
|
||
|
||
// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect
|
||
bool NavEKF2_core::getTouchdownExpected()
|
||
{
|
||
if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend.gndEffectTimeout_ms) {
|
||
expectGndEffectTouchdown = false;
|
||
}
|
||
|
||
return expectGndEffectTouchdown;
|
||
}
|
||
|
||
// called by vehicle code to specify that a touchdown is expected to happen
|
||
// causes the EKF to compensate for expected barometer errors due to ground effect
|
||
void NavEKF2_core::setTouchdownExpected(bool val)
|
||
{
|
||
touchdownExpectedSet_ms = imuSampleTime_ms;
|
||
expectGndEffectTouchdown = val;
|
||
}
|
||
|
||
// Monitor GPS data to see if quality is good enough to initialise the EKF
|
||
// Monitor magnetometer innovations to to see if the heading is good enough to use GPS
|
||
// Return true if all criteria pass for 10 seconds
|
||
bool NavEKF2_core::calcGpsGoodToAlign(void)
|
||
{
|
||
// calculate absolute difference between GPS vert vel and inertial vert vel
|
||
float velDiffAbs;
|
||
if (_ahrs->get_gps().have_vertical_velocity()) {
|
||
velDiffAbs = fabsf(gpsDataDelayed.vel.z - stateStruct.velocity.z);
|
||
} else {
|
||
velDiffAbs = 0.0f;
|
||
}
|
||
// fail if velocity difference or reported speed accuracy greater than threshold
|
||
bool gpsVelFail = (velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f);
|
||
// fail if not enough sats
|
||
bool numSatsFail = _ahrs->get_gps().num_sats() < 6;
|
||
// fail if horiziontal position accuracy not sufficient
|
||
float hAcc = 0.0f;
|
||
bool hAccFail;
|
||
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
|
||
hAccFail = hAcc > 5.0f;
|
||
} else {
|
||
hAccFail = false;
|
||
}
|
||
// fail if magnetometer innovations are outside limits indicating bad yaw
|
||
// with bad yaw we are unable to use GPS
|
||
bool yawFail;
|
||
if (magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) {
|
||
yawFail = true;
|
||
} else {
|
||
yawFail = false;
|
||
}
|
||
// record time of fail
|
||
// assume fail first time called
|
||
if (gpsVelFail || numSatsFail || hAccFail || yawFail || lastGpsVelFail_ms == 0) {
|
||
lastGpsVelFail_ms = imuSampleTime_ms;
|
||
}
|
||
// continuous period without fail required to return healthy
|
||
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// Read the range finder and take new measurements if available
|
||
// Read at 20Hz and apply a median filter
|
||
void NavEKF2_core::readRangeFinder(void)
|
||
{
|
||
uint8_t midIndex;
|
||
uint8_t maxIndex;
|
||
uint8_t minIndex;
|
||
// get theoretical correct range when the vehicle is on the ground
|
||
rngOnGnd = _rng.ground_clearance_cm() * 0.01f;
|
||
if (_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||
// store samples and sample time into a ring buffer
|
||
rngMeasIndex ++;
|
||
if (rngMeasIndex > 2) {
|
||
rngMeasIndex = 0;
|
||
}
|
||
storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms;
|
||
storedRngMeas[rngMeasIndex] = _rng.distance_cm() * 0.01f;
|
||
// check for three fresh samples and take median
|
||
bool sampleFresh[3];
|
||
for (uint8_t index = 0; index <= 2; index++) {
|
||
sampleFresh[index] = (imuSampleTime_ms - storedRngMeasTime_ms[index]) < 500;
|
||
}
|
||
if (sampleFresh[0] && sampleFresh[1] && sampleFresh[2]) {
|
||
if (storedRngMeas[0] > storedRngMeas[1]) {
|
||
minIndex = 1;
|
||
maxIndex = 0;
|
||
} else {
|
||
maxIndex = 0;
|
||
minIndex = 1;
|
||
}
|
||
if (storedRngMeas[2] > storedRngMeas[maxIndex]) {
|
||
midIndex = maxIndex;
|
||
} else if (storedRngMeas[2] < storedRngMeas[minIndex]) {
|
||
midIndex = minIndex;
|
||
} else {
|
||
midIndex = 2;
|
||
}
|
||
rngMea = max(storedRngMeas[midIndex],rngOnGnd);
|
||
newDataRng = true;
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
} else if (!filterArmed) {
|
||
// if not armed and no return, we assume on ground range
|
||
rngMea = rngOnGnd;
|
||
newDataRng = true;
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
} else {
|
||
newDataRng = false;
|
||
}
|
||
lastRngMeasTime_ms = imuSampleTime_ms;
|
||
}
|
||
}
|
||
|
||
// Detect takeoff for optical flow navigation
|
||
void NavEKF2_core::detectOptFlowTakeoff(void)
|
||
{
|
||
if (filterArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
Vector3f angRateVec;
|
||
Vector3f gyroBias;
|
||
getGyroBias(gyroBias);
|
||
bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1);
|
||
if (dual_ins) {
|
||
angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias;
|
||
} else {
|
||
angRateVec = ins.get_gyro() - gyroBias;
|
||
}
|
||
|
||
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f)));
|
||
}
|
||
}
|
||
|
||
// provides the height limit to be observed by the control loops
|
||
// returns false if no height limiting is required
|
||
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
|
||
bool NavEKF2_core::getHeightControlLimit(float &height) const
|
||
{
|
||
// only ask for limiting if we are doing optical flow navigation
|
||
if (frontend._fusionModeGPS == 3) {
|
||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||
height = max(float(_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f);
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
||
void NavEKF2_core::getQuaternion(Quaternion& ret) const
|
||
{
|
||
ret = outputDataNew.quat;
|
||
}
|
||
|
||
// align the NE earth magnetic field states with the published declination
|
||
void NavEKF2_core::alignMagStateDeclination()
|
||
{
|
||
// get the magnetic declination
|
||
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
|
||
|
||
// rotate the NE values so that the declination matches the published value
|
||
Vector3f initMagNED = stateStruct.earth_magfield;
|
||
float magLengthNE = pythagorous2(initMagNED.x,initMagNED.y);
|
||
stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng);
|
||
stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng);
|
||
}
|
||
|
||
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
||
// returns true if a reset yaw angle has been updated and not queried
|
||
// this function should not have more than one client
|
||
bool NavEKF2_core::getLastYawResetAngle(float &yawAng)
|
||
{
|
||
if (yawResetAngleWaiting) {
|
||
yawAng = yawResetAngle;
|
||
yawResetAngleWaiting = false;
|
||
return true;
|
||
} else {
|
||
yawAng = yawResetAngle;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states
|
||
void NavEKF2_core::fuseCompass()
|
||
{
|
||
float q0 = stateStruct.quat[0];
|
||
float q1 = stateStruct.quat[1];
|
||
float q2 = stateStruct.quat[2];
|
||
float q3 = stateStruct.quat[3];
|
||
|
||
float magX = magDataDelayed.mag.x;
|
||
float magY = magDataDelayed.mag.y;
|
||
float magZ = magDataDelayed.mag.z;
|
||
|
||
// compass measurement error variance (rad^2)
|
||
const float R_MAG = 3e-2f;
|
||
|
||
// Calculate observation Jacobian
|
||
float t2 = q0*q0;
|
||
float t3 = q1*q1;
|
||
float t4 = q2*q2;
|
||
float t5 = q3*q3;
|
||
float t6 = q0*q2*2.0f;
|
||
float t7 = q1*q3*2.0f;
|
||
float t8 = t6+t7;
|
||
float t9 = q0*q3*2.0f;
|
||
float t13 = q1*q2*2.0f;
|
||
float t10 = t9-t13;
|
||
float t11 = t2+t3-t4-t5;
|
||
float t12 = magX*t11;
|
||
float t14 = magZ*t8;
|
||
float t19 = magY*t10;
|
||
float t15 = t12+t14-t19;
|
||
float t16 = t2-t3+t4-t5;
|
||
float t17 = q0*q1*2.0f;
|
||
float t24 = q2*q3*2.0f;
|
||
float t18 = t17-t24;
|
||
float t20 = 1.0f/t15;
|
||
float t21 = magY*t16;
|
||
float t22 = t9+t13;
|
||
float t23 = magX*t22;
|
||
float t28 = magZ*t18;
|
||
float t25 = t21+t23-t28;
|
||
float t29 = t20*t25;
|
||
float t26 = tan(t29);
|
||
float t27 = 1.0f/(t15*t15);
|
||
float t30 = t26*t26;
|
||
float t31 = t30+1.0f;
|
||
float H_MAG[3];
|
||
H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
|
||
H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
|
||
H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
|
||
|
||
// 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] += P[rowIndex][colIndex]*H_MAG[colIndex];
|
||
}
|
||
varInnov += H_MAG[rowIndex]*PH[rowIndex];
|
||
}
|
||
float varInnovInv = 1.0f / varInnov;
|
||
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
|
||
Kfusion[rowIndex] = 0.0f;
|
||
for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
|
||
Kfusion[rowIndex] += P[rowIndex][colIndex]*H_MAG[colIndex];
|
||
}
|
||
Kfusion[rowIndex] *= varInnovInv;
|
||
}
|
||
|
||
// Calculate the innovation
|
||
float innovation = calcMagHeadingInnov();
|
||
|
||
// Copy raw value to output variable used for data logging
|
||
innovYaw = innovation;
|
||
|
||
// 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
|
||
stateStruct.angErr.zero();
|
||
for (uint8_t i=0; i<=stateIndexLim; i++) {
|
||
statesArray[i] -= Kfusion[i] * innovation;
|
||
}
|
||
|
||
// the first 3 states represent the angular misalignment vector. This is
|
||
// is used to correct the estimated quaternion on the current time step
|
||
stateStruct.quat.rotate(stateStruct.angErr);
|
||
|
||
// 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[24];
|
||
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++) {
|
||
HP[colIndex] = 0.0f;
|
||
for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
|
||
HP[colIndex] += H_MAG[rowIndex]*P[rowIndex][colIndex];
|
||
}
|
||
}
|
||
for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
|
||
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++) {
|
||
P[rowIndex][colIndex] -= Kfusion[rowIndex] * HP[colIndex];
|
||
}
|
||
}
|
||
|
||
// force the covariance matrix to be symmetrical and limit the variances to prevent
|
||
// ill-condiioning.
|
||
ForceSymmetry();
|
||
ConstrainVariances();
|
||
}
|
||
|
||
// Calculate magnetic heading innovation
|
||
float NavEKF2_core::calcMagHeadingInnov()
|
||
{
|
||
// rotate predicted earth components into body axes and calculate
|
||
// predicted measurements
|
||
Matrix3f Tbn_temp;
|
||
stateStruct.quat.rotation_matrix(Tbn_temp);
|
||
Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag;
|
||
|
||
// calculate the innovation where the predicted measurement is the angle wrt magnetic north of the horizontal component of the measured field
|
||
float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination();
|
||
|
||
// wrap the innovation so it sits on the range from +-pi
|
||
if (innovation > 3.1415927f) {
|
||
innovation = innovation - 6.2831853f;
|
||
} else if (innovation < -3.1415927f) {
|
||
innovation = innovation + 6.2831853f;
|
||
}
|
||
|
||
return innovation;
|
||
}
|
||
|
||
#endif // HAL_CPU_CLASS
|