mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
39969e7d8e
AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
718 lines
34 KiB
C++
718 lines
34 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
#include "AP_NavEKF3.h"
|
|
#include "AP_NavEKF3_core.h"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/********************************************************
|
|
* RESET FUNCTIONS *
|
|
********************************************************/
|
|
|
|
/********************************************************
|
|
* FUSE MEASURED_DATA *
|
|
********************************************************/
|
|
|
|
// select fusion of optical flow measurements
|
|
void NavEKF3_core::SelectFlowFusion()
|
|
{
|
|
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
|
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
|
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
|
if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
|
|
optFlowFusionDelayed = true;
|
|
return;
|
|
} else {
|
|
optFlowFusionDelayed = false;
|
|
}
|
|
|
|
// start performance timer
|
|
hal.util->perf_begin(_perf_FuseOptFlow);
|
|
// Perform Data Checks
|
|
// Check if the optical flow data is still valid
|
|
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
|
|
// check is the terrain offset estimate is still valid
|
|
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
|
|
// Perform tilt check
|
|
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
|
|
// Constrain measurements to zero if takeoff is not detected and the height above ground
|
|
// is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
|
|
// and carried to test optical flow operation
|
|
if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
|
|
ofDataDelayed.flowRadXYcomp.zero();
|
|
ofDataDelayed.flowRadXY.zero();
|
|
flowDataValid = true;
|
|
}
|
|
|
|
// 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 ((flowDataToFuse || rangeDataToFuse) && tiltOK) {
|
|
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
|
|
fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse);
|
|
// Estimate the terrain offset (runs a one state EKF)
|
|
EstimateTerrainOffset();
|
|
}
|
|
|
|
// Fuse optical flow data into the main filter
|
|
if (flowDataToFuse && tiltOK)
|
|
{
|
|
// Set the flow noise used by the fusion processes
|
|
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
|
|
// 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
|
|
flowDataToFuse = false;
|
|
}
|
|
|
|
// stop the performance timer
|
|
hal.util->perf_end(_perf_FuseOptFlow);
|
|
}
|
|
|
|
/*
|
|
Estimation of terrain offset using a single state EKF
|
|
The filter can fuse motion compensated optiocal flow rates and range finder measurements
|
|
*/
|
|
void NavEKF3_core::EstimateTerrainOffset()
|
|
{
|
|
// start performance timer
|
|
hal.util->perf_begin(_perf_TerrainOffset);
|
|
|
|
// 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
|
|
// don't update terrain state if not generating enough LOS rate, or without GPS, as it is poorly observable
|
|
// don't update terrain state if we are using it as a height reference in the main filter
|
|
bool cantFuseFlowData = (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f);
|
|
if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) {
|
|
// skip update
|
|
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 the growth in uncertainty due to the copters vertical velocity
|
|
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
|
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[6][6];
|
|
Popt += Pincrement;
|
|
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
|
|
|
// fuse range finder data
|
|
if (rangeDataToFuse) {
|
|
// predict range
|
|
float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.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 - rangeDataDelayed.rng;
|
|
|
|
// calculate the innovation consistency test ratio
|
|
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * 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 && !cantFuseFlowData) {
|
|
|
|
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;
|
|
|
|
// predict range to centre of image
|
|
float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
|
|
|
|
// constrain terrain height to be below the vehicle
|
|
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
|
|
|
|
// calculate relative velocity in sensor frame
|
|
relVelSensor = prevTnb*stateStruct.velocity;
|
|
|
|
// 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*stateStruct.velocity.x;
|
|
float t16 = t10+t11;
|
|
float t17 = t16*stateStruct.velocity.y;
|
|
float t18 = q0*q2*2.0f;
|
|
float t19 = q1*q3*2.0f;
|
|
float t20 = t18-t19;
|
|
float t21 = t20*stateStruct.velocity.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*stateStruct.velocity.y;
|
|
float t26 = t10-t11;
|
|
float t27 = t26*stateStruct.velocity.x;
|
|
float t28 = q0*q1*2.0f;
|
|
float t29 = q2*q3*2.0f;
|
|
float t30 = t28+t29;
|
|
float t31 = t30*stateStruct.velocity.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(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * 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
|
|
hal.util->perf_end(_perf_TerrainOffset);
|
|
}
|
|
|
|
/*
|
|
* Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox.
|
|
* The script file used to generate these and other equations in this filter can be found here:
|
|
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
|
|
* Requires a valid terrain height estimate.
|
|
*/
|
|
void NavEKF3_core::FuseOptFlow()
|
|
{
|
|
Vector24 H_LOS;
|
|
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;
|
|
|
|
// 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/prevTnb.c.z),rngOnGnd,1000.0f);
|
|
|
|
// correct range for flow sensor offset body frame position offset
|
|
// the corrected value is the predicted range from the sensor focal point to the
|
|
// centre of the image on the ground assuming flat terrain
|
|
Vector3f posOffsetBody = (*ofDataDelayed.body_offset) - accelPosOffset;
|
|
if (!posOffsetBody.is_zero()) {
|
|
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
|
range -= posOffsetEarth.z / prevTnb.c.z;
|
|
}
|
|
|
|
// calculate relative velocity in sensor frame including the relative motion due to rotation
|
|
relVelSensor = (prevTnb * stateStruct.velocity) + (ofDataDelayed.bodyRadXYZ % posOffsetBody);
|
|
|
|
// 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) {
|
|
// calculate X axis observation Jacobian
|
|
float t2 = 1.0f / range;
|
|
H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
|
H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
|
H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
|
H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
|
H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
|
H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
|
H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
|
|
|
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
|
float t3 = q1*vd*2.0f;
|
|
float t4 = q0*ve*2.0f;
|
|
float t11 = q3*vn*2.0f;
|
|
float t5 = t3+t4-t11;
|
|
float t6 = q0*q3*2.0f;
|
|
float t29 = q1*q2*2.0f;
|
|
float t7 = t6-t29;
|
|
float t8 = q0*q1*2.0f;
|
|
float t9 = q2*q3*2.0f;
|
|
float t10 = t8+t9;
|
|
float t12 = P[0][0]*t2*t5;
|
|
float t13 = q0*vd*2.0f;
|
|
float t14 = q2*vn*2.0f;
|
|
float t28 = q1*ve*2.0f;
|
|
float t15 = t13+t14-t28;
|
|
float t16 = q3*vd*2.0f;
|
|
float t17 = q2*ve*2.0f;
|
|
float t18 = q1*vn*2.0f;
|
|
float t19 = t16+t17+t18;
|
|
float t20 = q3*ve*2.0f;
|
|
float t21 = q0*vn*2.0f;
|
|
float t30 = q2*vd*2.0f;
|
|
float t22 = t20+t21-t30;
|
|
float t23 = q0*q0;
|
|
float t24 = q1*q1;
|
|
float t25 = q2*q2;
|
|
float t26 = q3*q3;
|
|
float t27 = t23-t24+t25-t26;
|
|
float t31 = P[1][1]*t2*t15;
|
|
float t32 = P[6][0]*t2*t10;
|
|
float t33 = P[1][0]*t2*t15;
|
|
float t34 = P[2][0]*t2*t19;
|
|
float t35 = P[5][0]*t2*t27;
|
|
float t79 = P[4][0]*t2*t7;
|
|
float t80 = P[3][0]*t2*t22;
|
|
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
|
float t37 = t2*t5*t36;
|
|
float t38 = P[6][1]*t2*t10;
|
|
float t39 = P[0][1]*t2*t5;
|
|
float t40 = P[2][1]*t2*t19;
|
|
float t41 = P[5][1]*t2*t27;
|
|
float t81 = P[4][1]*t2*t7;
|
|
float t82 = P[3][1]*t2*t22;
|
|
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
|
float t43 = t2*t15*t42;
|
|
float t44 = P[6][2]*t2*t10;
|
|
float t45 = P[0][2]*t2*t5;
|
|
float t46 = P[1][2]*t2*t15;
|
|
float t47 = P[2][2]*t2*t19;
|
|
float t48 = P[5][2]*t2*t27;
|
|
float t83 = P[4][2]*t2*t7;
|
|
float t84 = P[3][2]*t2*t22;
|
|
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
|
float t50 = t2*t19*t49;
|
|
float t51 = P[6][3]*t2*t10;
|
|
float t52 = P[0][3]*t2*t5;
|
|
float t53 = P[1][3]*t2*t15;
|
|
float t54 = P[2][3]*t2*t19;
|
|
float t55 = P[5][3]*t2*t27;
|
|
float t85 = P[4][3]*t2*t7;
|
|
float t86 = P[3][3]*t2*t22;
|
|
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
|
float t57 = P[6][5]*t2*t10;
|
|
float t58 = P[0][5]*t2*t5;
|
|
float t59 = P[1][5]*t2*t15;
|
|
float t60 = P[2][5]*t2*t19;
|
|
float t61 = P[5][5]*t2*t27;
|
|
float t88 = P[4][5]*t2*t7;
|
|
float t89 = P[3][5]*t2*t22;
|
|
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
|
float t63 = t2*t27*t62;
|
|
float t64 = P[6][4]*t2*t10;
|
|
float t65 = P[0][4]*t2*t5;
|
|
float t66 = P[1][4]*t2*t15;
|
|
float t67 = P[2][4]*t2*t19;
|
|
float t68 = P[5][4]*t2*t27;
|
|
float t90 = P[4][4]*t2*t7;
|
|
float t91 = P[3][4]*t2*t22;
|
|
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
|
float t70 = P[6][6]*t2*t10;
|
|
float t71 = P[0][6]*t2*t5;
|
|
float t72 = P[1][6]*t2*t15;
|
|
float t73 = P[2][6]*t2*t19;
|
|
float t74 = P[5][6]*t2*t27;
|
|
float t93 = P[4][6]*t2*t7;
|
|
float t94 = P[3][6]*t2*t22;
|
|
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
|
float t76 = t2*t10*t75;
|
|
float t87 = t2*t22*t56;
|
|
float t92 = t2*t7*t69;
|
|
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
|
float t78;
|
|
|
|
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
|
if (t77 > R_LOS) {
|
|
t78 = 1.0f/t77;
|
|
faultStatus.bad_xflow = false;
|
|
} else {
|
|
t77 = R_LOS;
|
|
t78 = 1.0f/R_LOS;
|
|
faultStatus.bad_xflow = true;
|
|
return;
|
|
}
|
|
varInnovOptFlow[0] = t77;
|
|
|
|
// calculate innovation for X axis observation
|
|
innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
|
|
|
|
// calculate Kalman gains for X-axis observation
|
|
Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
|
Kfusion[1] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
|
Kfusion[2] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
|
Kfusion[3] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
|
Kfusion[4] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
|
Kfusion[5] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
|
Kfusion[6] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
|
Kfusion[7] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
|
Kfusion[8] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
|
Kfusion[9] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
|
Kfusion[10] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
|
Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
|
Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
|
Kfusion[13] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
|
Kfusion[14] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
|
Kfusion[15] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
|
Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
|
} else {
|
|
Kfusion[22] = 0.0f;
|
|
Kfusion[23] = 0.0f;
|
|
}
|
|
if (!inhibitMagStates) {
|
|
Kfusion[16] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
|
Kfusion[17] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
|
Kfusion[18] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
|
Kfusion[19] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
|
Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
|
Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
|
} else {
|
|
for (uint8_t i = 16; i <= 21; i++) {
|
|
Kfusion[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
|
|
// calculate Y axis observation Jacobian
|
|
float t2 = 1.0f / range;
|
|
H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
|
H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
|
H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
|
H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
|
H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
|
H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
|
H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
|
|
|
// calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains
|
|
float t3 = q3*ve*2.0f;
|
|
float t4 = q0*vn*2.0f;
|
|
float t11 = q2*vd*2.0f;
|
|
float t5 = t3+t4-t11;
|
|
float t6 = q0*q3*2.0f;
|
|
float t7 = q1*q2*2.0f;
|
|
float t8 = t6+t7;
|
|
float t9 = q0*q2*2.0f;
|
|
float t28 = q1*q3*2.0f;
|
|
float t10 = t9-t28;
|
|
float t12 = P[0][0]*t2*t5;
|
|
float t13 = q3*vd*2.0f;
|
|
float t14 = q2*ve*2.0f;
|
|
float t15 = q1*vn*2.0f;
|
|
float t16 = t13+t14+t15;
|
|
float t17 = q0*vd*2.0f;
|
|
float t18 = q2*vn*2.0f;
|
|
float t29 = q1*ve*2.0f;
|
|
float t19 = t17+t18-t29;
|
|
float t20 = q1*vd*2.0f;
|
|
float t21 = q0*ve*2.0f;
|
|
float t30 = q3*vn*2.0f;
|
|
float t22 = t20+t21-t30;
|
|
float t23 = q0*q0;
|
|
float t24 = q1*q1;
|
|
float t25 = q2*q2;
|
|
float t26 = q3*q3;
|
|
float t27 = t23+t24-t25-t26;
|
|
float t31 = P[1][1]*t2*t16;
|
|
float t32 = P[5][0]*t2*t8;
|
|
float t33 = P[1][0]*t2*t16;
|
|
float t34 = P[3][0]*t2*t22;
|
|
float t35 = P[4][0]*t2*t27;
|
|
float t80 = P[6][0]*t2*t10;
|
|
float t81 = P[2][0]*t2*t19;
|
|
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
|
float t37 = t2*t5*t36;
|
|
float t38 = P[5][1]*t2*t8;
|
|
float t39 = P[0][1]*t2*t5;
|
|
float t40 = P[3][1]*t2*t22;
|
|
float t41 = P[4][1]*t2*t27;
|
|
float t82 = P[6][1]*t2*t10;
|
|
float t83 = P[2][1]*t2*t19;
|
|
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
|
float t43 = t2*t16*t42;
|
|
float t44 = P[5][2]*t2*t8;
|
|
float t45 = P[0][2]*t2*t5;
|
|
float t46 = P[1][2]*t2*t16;
|
|
float t47 = P[3][2]*t2*t22;
|
|
float t48 = P[4][2]*t2*t27;
|
|
float t79 = P[2][2]*t2*t19;
|
|
float t84 = P[6][2]*t2*t10;
|
|
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
|
float t50 = P[5][3]*t2*t8;
|
|
float t51 = P[0][3]*t2*t5;
|
|
float t52 = P[1][3]*t2*t16;
|
|
float t53 = P[3][3]*t2*t22;
|
|
float t54 = P[4][3]*t2*t27;
|
|
float t86 = P[6][3]*t2*t10;
|
|
float t87 = P[2][3]*t2*t19;
|
|
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
|
float t56 = t2*t22*t55;
|
|
float t57 = P[5][4]*t2*t8;
|
|
float t58 = P[0][4]*t2*t5;
|
|
float t59 = P[1][4]*t2*t16;
|
|
float t60 = P[3][4]*t2*t22;
|
|
float t61 = P[4][4]*t2*t27;
|
|
float t88 = P[6][4]*t2*t10;
|
|
float t89 = P[2][4]*t2*t19;
|
|
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
|
float t63 = t2*t27*t62;
|
|
float t64 = P[5][5]*t2*t8;
|
|
float t65 = P[0][5]*t2*t5;
|
|
float t66 = P[1][5]*t2*t16;
|
|
float t67 = P[3][5]*t2*t22;
|
|
float t68 = P[4][5]*t2*t27;
|
|
float t90 = P[6][5]*t2*t10;
|
|
float t91 = P[2][5]*t2*t19;
|
|
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
|
float t70 = t2*t8*t69;
|
|
float t71 = P[5][6]*t2*t8;
|
|
float t72 = P[0][6]*t2*t5;
|
|
float t73 = P[1][6]*t2*t16;
|
|
float t74 = P[3][6]*t2*t22;
|
|
float t75 = P[4][6]*t2*t27;
|
|
float t92 = P[6][6]*t2*t10;
|
|
float t93 = P[2][6]*t2*t19;
|
|
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
|
float t85 = t2*t19*t49;
|
|
float t94 = t2*t10*t76;
|
|
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
|
float t78;
|
|
|
|
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
|
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
|
if (t77 > R_LOS) {
|
|
t78 = 1.0f/t77;
|
|
faultStatus.bad_yflow = false;
|
|
} else {
|
|
t77 = R_LOS;
|
|
t78 = 1.0f/R_LOS;
|
|
faultStatus.bad_yflow = true;
|
|
return;
|
|
}
|
|
varInnovOptFlow[1] = t77;
|
|
|
|
// calculate innovation for Y observation
|
|
innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
|
|
|
|
// calculate Kalman gains for the Y-axis observation
|
|
Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
|
Kfusion[1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
|
Kfusion[2] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
|
Kfusion[3] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
|
Kfusion[4] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
|
Kfusion[5] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
|
Kfusion[6] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
|
Kfusion[7] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
|
Kfusion[8] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
|
Kfusion[9] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
|
Kfusion[10] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
|
Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
|
Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
|
Kfusion[13] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
|
Kfusion[14] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
|
Kfusion[15] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
|
if (!inhibitWindStates) {
|
|
Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
|
Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
|
} else {
|
|
Kfusion[22] = 0.0f;
|
|
Kfusion[23] = 0.0f;
|
|
}
|
|
if (!inhibitMagStates) {
|
|
Kfusion[16] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
|
Kfusion[17] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
|
Kfusion[18] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
|
Kfusion[19] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
|
Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
|
Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
|
} 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(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * 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;
|
|
|
|
// correct the covariance P = (I - K*H)*P
|
|
// take advantage of the empty columns in KH to reduce the
|
|
// number of operations
|
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
|
for (unsigned j = 0; j<=6; j++) {
|
|
KH[i][j] = Kfusion[i] * H_LOS[j];
|
|
}
|
|
for (unsigned j = 7; j<=stateIndexLim; j++) {
|
|
KH[i][j] = 0.0f;
|
|
}
|
|
}
|
|
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
|
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
|
ftype res = 0;
|
|
res += KH[i][0] * P[0][j];
|
|
res += KH[i][1] * P[1][j];
|
|
res += KH[i][2] * P[2][j];
|
|
res += KH[i][3] * P[3][j];
|
|
res += KH[i][4] * P[4][j];
|
|
res += KH[i][5] * P[5][j];
|
|
res += KH[i][6] * P[6][j];
|
|
KHP[i][j] = res;
|
|
}
|
|
}
|
|
|
|
// Check that we are not going to drive any variances negative and skip the update if so
|
|
bool healthyFusion = true;
|
|
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
|
if (KHP[i][i] > P[i][i]) {
|
|
healthyFusion = false;
|
|
}
|
|
}
|
|
|
|
if (healthyFusion) {
|
|
// update the covariance matrix
|
|
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();
|
|
|
|
// correct the state vector
|
|
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
|
statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
|
}
|
|
stateStruct.quat.normalize();
|
|
|
|
} else {
|
|
// record bad axis
|
|
if (obsIndex == 0) {
|
|
faultStatus.bad_xflow = true;
|
|
} else if (obsIndex == 1) {
|
|
faultStatus.bad_yflow = true;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/********************************************************
|
|
* MISC FUNCTIONS *
|
|
********************************************************/
|
|
|
|
#endif // HAL_CPU_CLASS
|