mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: split up EKF_core into different files
This commit is contained in:
Siddharth Bharat Purohit
Randy Mackay
parent
b5abab9d37
commit
290ea0e1e8
|
|
@ -0,0 +1,422 @@
|
|||
/// -*- 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;
|
||||
|
||||
// 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 (onGround && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// all OK
|
||||
return true;
|
||||
}
|
||||
|
||||
// Control filter mode transitions
|
||||
void NavEKF2_core::controlFilterModes()
|
||||
{
|
||||
// Determine motor arm status
|
||||
prevMotorsArmed = motorsArmed;
|
||||
motorsArmed = hal.util->get_soft_armed();
|
||||
if (motorsArmed && !prevMotorsArmed) {
|
||||
// set the time at which we arm to assist with checks
|
||||
timeAtArming_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// Detect if we are in flight on or ground
|
||||
detectFlight();
|
||||
|
||||
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
|
||||
// avoid unnecessary operations
|
||||
setWindMagStateLearningMode();
|
||||
|
||||
// Check the alignmnent status of the tilt and yaw attitude
|
||||
// Used during initial bootstrap alignment of the filter
|
||||
checkAttitudeAlignmentStatus();
|
||||
|
||||
// Control reset of yaw and magnetic field states
|
||||
controlMagYawReset();
|
||||
|
||||
// Set the type of inertial navigation aiding used
|
||||
setAidingMode();
|
||||
|
||||
}
|
||||
|
||||
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
|
||||
// avoid unnecessary operations
|
||||
void NavEKF2_core::setWindMagStateLearningMode()
|
||||
{
|
||||
// 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 || (PV_AidingMode == AID_NONE));
|
||||
|
||||
// determine if the vehicle is manoevring
|
||||
if (accNavMagHoriz > 0.5f) {
|
||||
manoeuvring = true;
|
||||
} else {
|
||||
manoeuvring = false;
|
||||
}
|
||||
|
||||
// Determine if learning of magnetic field states has been requested by the user
|
||||
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() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
|
||||
|
||||
// Inhibit the magnetic field calibration if not requested or denied
|
||||
inhibitMagStates = (!magCalRequested || magCalDenied);
|
||||
|
||||
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
|
||||
// if we are not using those states
|
||||
if (inhibitMagStates && inhibitWindStates) {
|
||||
stateIndexLim = 15;
|
||||
} else if (inhibitWindStates) {
|
||||
stateIndexLim = 21;
|
||||
} else {
|
||||
stateIndexLim = 23;
|
||||
}
|
||||
}
|
||||
|
||||
// Set inertial navigation aiding mode
|
||||
void NavEKF2_core::setAidingMode()
|
||||
{
|
||||
// Determine when to commence aiding for inertial navigation
|
||||
// Save the previous status so we can detect when it has changed
|
||||
prevIsAiding = isAiding;
|
||||
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
||||
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
|
||||
// If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
|
||||
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
|
||||
// Latch to on. Aiding can be turned off by setting both
|
||||
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
|
||||
|
||||
// check to see if we are starting or stopping aiding and set states and modes as required
|
||||
if (isAiding != prevIsAiding) {
|
||||
// We have transitioned either into or out of aiding
|
||||
// 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 when we start aiding. These are then held until aiding is stopped.
|
||||
if (!isAiding) {
|
||||
// We have ceased aiding
|
||||
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
|
||||
PV_AidingMode = AID_NONE;
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
// store the current position to be used to keep reporting the last known position
|
||||
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 baro noise filter to settle before the filtered baro data 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) {
|
||||
// We have commenced aiding, but GPS useage has been prohibited so use optical flow only
|
||||
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;
|
||||
// Reset the last valid flow measurement time
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
// Reset the last valid flow fusion time
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
} else {
|
||||
// We have commenced aiding and GPS useage is allowed
|
||||
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;
|
||||
// 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 all position, velocity and covariance
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
CovarianceInit();
|
||||
|
||||
}
|
||||
|
||||
// Always turn aiding off when the vehicle is disarmed
|
||||
if (!isAiding) {
|
||||
PV_AidingMode = AID_NONE;
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check the alignmnent status of the tilt and yaw attitude
|
||||
// Used during initial bootstrap alignment of the filter
|
||||
void NavEKF2_core::checkAttitudeAlignmentStatus()
|
||||
{
|
||||
// Check for tilt convergence - used during initial alignment
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
// 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 && gpsQualGood;
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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 = (PV_AidingMode == AID_ABSOLUTE);
|
||||
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 = 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 = (PV_AidingMode == AID_NONE) && 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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
|
@ -0,0 +1,790 @@
|
|||
/// -*- 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;
|
||||
|
||||
/********************************************************
|
||||
* RESET FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// Control reset of yaw and magnetic field states
|
||||
void NavEKF2_core::controlMagYawReset()
|
||||
{
|
||||
// Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
|
||||
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
|
||||
if (inFlight && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
|
||||
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after commencement of flight
|
||||
Vector3f eulerAngles;
|
||||
getEulerAngles(eulerAngles);
|
||||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
StoreQuatReset();
|
||||
firstMagYawInit = true;
|
||||
} else if (inFlight && !secondMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f) {
|
||||
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after commencement of flight
|
||||
// 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;
|
||||
}
|
||||
|
||||
// perform a yaw alignment check against GPS if exiting on-ground mode for fly forward type vehicle (plane)
|
||||
// this is done to protect against unrecoverable heading alignment errors due to compass faults
|
||||
if (!onGround && prevOnGround && assume_zero_sideslip()) {
|
||||
alignYawGPS();
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* GET STATES/PARAMS FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// 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;
|
||||
}
|
||||
/********************************************************
|
||||
* SET STATES/PARAMS FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
|
||||
/********************************************************
|
||||
* READ SENSORS *
|
||||
********************************************************/
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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 for consistent data between magnetometers
|
||||
consistentMagData = _ahrs->get_compass()->consistent();
|
||||
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* FUSE MEASURED_DATA *
|
||||
********************************************************/
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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 && (PV_AidingMode == AID_ABSOLUTE)) {
|
||||
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 (motorsArmed && ((PV_AidingMode == AID_NONE) || 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 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 > M_PI) {
|
||||
innovation = innovation - 2*M_PI;
|
||||
} else if (innovation < -M_PI) {
|
||||
innovation = innovation + 2*M_PI;
|
||||
}
|
||||
|
||||
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
|
||||
if (innovation - lastInnovation > M_PI) {
|
||||
// Angle has wrapped in the positive direction to subtract an additional 2*Pi
|
||||
innovationIncrement -= 2*M_PI;
|
||||
} else if (innovation -innovationIncrement < -M_PI) {
|
||||
// Angle has wrapped in the negative direction so add an additional 2*Pi
|
||||
innovationIncrement += 2*M_PI;
|
||||
}
|
||||
lastInnovation = innovation;
|
||||
|
||||
return innovation + innovationIncrement;
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* MISC FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,284 @@
|
|||
/// -*- 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;
|
||||
|
||||
|
||||
/* 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
|
||||
|
||||
We also record the failure reason so that prearm_failure_reason()
|
||||
can give a good report to the user on why arming is failing
|
||||
*/
|
||||
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);
|
||||
|
||||
if (velDiffAbs > 1.0f) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS vert vel error %.1f", (double)velDiffAbs);
|
||||
}
|
||||
if (gpsSpdAccuracy > 1.0f) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS speed error %.1f", (double)gpsSpdAccuracy);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
if (hAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS horiz error %.1f", (double)hAcc);
|
||||
}
|
||||
|
||||
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
|
||||
// This enables us to handle large changes to the external magnetic field environment that occur before arming
|
||||
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) || !consistentMagData) {
|
||||
magYawResetTimer_ms = imuSampleTime_ms;
|
||||
}
|
||||
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
|
||||
// reset heading and field states
|
||||
Vector3f eulerAngles;
|
||||
getEulerAngles(eulerAngles);
|
||||
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
|
||||
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
|
||||
magYawResetTimer_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
if (yawFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"Mag yaw error x=%.1f y=%.1f",
|
||||
(double)magTestRatio.x,
|
||||
(double)magTestRatio.y);
|
||||
}
|
||||
|
||||
// fail if not enough sats
|
||||
bool numSatsFail = _ahrs->get_gps().num_sats() < 6;
|
||||
if (numSatsFail) {
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
|
||||
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
|
||||
}
|
||||
|
||||
// record time of fail if failing
|
||||
if (gpsVelFail || numSatsFail || hAccFail || yawFail) {
|
||||
lastGpsVelFail_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
if (lastGpsVelFail_ms == 0) {
|
||||
// first time through, start with a failure
|
||||
lastGpsVelFail_ms = imuSampleTime_ms;
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF warmup");
|
||||
}
|
||||
|
||||
// DEBUG PRINT
|
||||
//hal.console->printf("velDiff = %5.2f, nSats = %i, hAcc = %5.2f, sAcc = %5.2f, delTime = %i\n", velDiffAbs, _ahrs->get_gps().num_sats(), hAcc, gpsSpdAccuracy, imuSampleTime_ms - lastGpsVelFail_ms);
|
||||
// continuous period without fail required to return healthy
|
||||
|
||||
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
|
||||
// we have not failed in the last 10 seconds
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Detect if we are in flight or on ground
|
||||
void NavEKF2_core::detectFlight()
|
||||
{
|
||||
/*
|
||||
If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria.
|
||||
Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where
|
||||
onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for
|
||||
both onGround and inFlight to be false if the status is uncertain, but they cannot both be true.
|
||||
|
||||
If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used
|
||||
|
||||
TODO - this logic should be moved out of the EKF and into the flight vehicle code.
|
||||
*/
|
||||
|
||||
if (assume_zero_sideslip()) {
|
||||
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
|
||||
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;
|
||||
}
|
||||
|
||||
// Determine to a high certainty we are flying
|
||||
if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {
|
||||
onGround = false;
|
||||
inFlight = true;
|
||||
}
|
||||
|
||||
// if is possible we are in flight, set the time this condition was last detected
|
||||
if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) {
|
||||
airborneDetectTime_ms = imuSampleTime_ms;
|
||||
onGround = false;
|
||||
}
|
||||
|
||||
// Determine if is is possible we are on the ground
|
||||
if (highGndSpd || highAirSpd || largeHgtChange) {
|
||||
inFlight = false;
|
||||
}
|
||||
|
||||
// Determine to a high certainty we are not flying
|
||||
// after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode
|
||||
if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
|
||||
onGround = true;
|
||||
inFlight = false;
|
||||
}
|
||||
} else {
|
||||
// Non fly forward vehicle, so can only use height and motor arm status
|
||||
|
||||
// If the motors are armed then we could be flying and if they are not armed then we are definitely not flying
|
||||
if (motorsArmed) {
|
||||
onGround = false;
|
||||
} else {
|
||||
inFlight = false;
|
||||
onGround = true;
|
||||
}
|
||||
|
||||
// If height has increased since exiting on-ground, then we definitely are flying
|
||||
if (!onGround && ((stateStruct.position.z - posDownAtTakeoff) < -1.5f)) {
|
||||
inFlight = true;
|
||||
}
|
||||
|
||||
// If rangefinder has increased since exiting on-ground, then we definitely are flying
|
||||
if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) {
|
||||
inFlight = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// store current on-ground and in-air status for next time
|
||||
prevOnGround = onGround;
|
||||
prevInFlight = inFlight;
|
||||
|
||||
// Store vehicle height and range prior to takeoff for use in post takeoff checks
|
||||
if (!onGround && !prevOnGround) {
|
||||
// store vertical position at start of flight to use as a reference for ground relative checks
|
||||
posDownAtTakeoff = stateStruct.position.z;
|
||||
// store the range finder measurement which will be used as a reference to detect when we have taken off
|
||||
rngAtStartOfFlight = rngMea;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Detect takeoff for optical flow navigation
|
||||
void NavEKF2_core::detectOptFlowTakeoff(void)
|
||||
{
|
||||
if (motorsArmed && !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 > (rngAtStartOfFlight + 0.1f)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
|
@ -0,0 +1,509 @@
|
|||
/// -*- 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;
|
||||
|
||||
/********************************************************
|
||||
* RESET FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
|
||||
|
||||
/********************************************************
|
||||
* GET STATES/PARAMS FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* SET STATES/PARAMS FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
/********************************************************
|
||||
* READ SENSORS *
|
||||
********************************************************/
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* FUSE MEASURED_DATA *
|
||||
********************************************************/
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// store TAS in a history array
|
||||
void NavEKF2_core::StoreTAS()
|
||||
{
|
||||
if (tasStoreIndex >= OBS_BUFFER_LENGTH) {
|
||||
tasStoreIndex = 0;
|
||||
}
|
||||
storedTAS[tasStoreIndex] = tasDataNew;
|
||||
tasStoreIndex += 1;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* MISC FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,87 @@
|
|||
/// -*- 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;
|
||||
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
// 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 error as a percentage
|
||||
void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
|
||||
{
|
||||
if (!statesInitialised) {
|
||||
gyroScale.x = gyroScale.y = gyroScale.z = 0;
|
||||
return;
|
||||
}
|
||||
gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
|
||||
gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
|
||||
gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.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];
|
||||
}
|
||||
|
||||
/*
|
||||
vehicle specific initial gyro bias uncertainty in deg/sec
|
||||
*/
|
||||
float NavEKF2_core::InitialGyroBiasUncertainty(void) const
|
||||
{
|
||||
return 5.0f;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
Loading…
Reference in New Issue