AP_NavEKF3: added EKF3 for EKF experimentation
AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
This commit is contained in:
priseborough
Andrew Tridgell
parent
ab05472e0e
commit
39969e7d8e
1404
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Normal file
1404
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Normal file
File diff suppressed because it is too large
Load Diff
433
libraries/AP_NavEKF3/AP_NavEKF3.h
Normal file
433
libraries/AP_NavEKF3/AP_NavEKF3.h
Normal file
@ -0,0 +1,433 @@
|
||||
/*
|
||||
24 state EKF based on https://github.com/priseborough/InertialNav
|
||||
Converted from Matlab to C++ by Paul Riseborough
|
||||
|
||||
EKF Tuning parameters refactored by Tom Cauchois
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_NavEKF/AP_Nav_Common.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
|
||||
class NavEKF3_core;
|
||||
class AP_AHRS;
|
||||
|
||||
class NavEKF3
|
||||
{
|
||||
public:
|
||||
friend class NavEKF3_core;
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
NavEKF3(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng);
|
||||
|
||||
// allow logging to determine the number of active cores
|
||||
uint8_t activeCores(void) const {
|
||||
return num_cores;
|
||||
}
|
||||
|
||||
// Initialise the filter
|
||||
bool InitialiseFilter(void);
|
||||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void UpdateFilter(void);
|
||||
|
||||
// check if we should write log messages
|
||||
void check_log_write(void);
|
||||
|
||||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool healthy(void) const;
|
||||
|
||||
// returns the index of the primary core
|
||||
// return -1 if no primary core selected
|
||||
int8_t getPrimaryCoreIndex(void) const;
|
||||
|
||||
// returns the index of the IMU of the primary core
|
||||
// return -1 if no primary core selected
|
||||
int8_t getPrimaryCoreIMUIndex(void) const;
|
||||
|
||||
// Write the last calculated NE position relative to the reference point (m) for the specified instance.
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNE(int8_t instance, Vector2f &posNE);
|
||||
|
||||
// Write the last calculated D position relative to the reference point (m) for the specified instance.
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosD(int8_t instance, float &posD);
|
||||
|
||||
// return NED velocity in m/s for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getVelNED(int8_t instance, Vector3f &vel);
|
||||
|
||||
// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
// This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
|
||||
// but will always be kinematically consistent with the z component of the EKF position state
|
||||
float getPosDownDerivative(int8_t instance);
|
||||
|
||||
// This returns the specific forces in the NED frame
|
||||
void getAccelNED(Vector3f &accelNED) const;
|
||||
|
||||
// return body axis gyro bias estimates in rad/sec for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getGyroBias(int8_t instance, Vector3f &gyroBias);
|
||||
|
||||
// return accelerometer bias estimate in m/s/s
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getAccelBias(int8_t instance, Vector3f &accelBias);
|
||||
|
||||
// return tilt error convergence metric for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getTiltError(int8_t instance, float &ang);
|
||||
|
||||
// reset body axis gyro bias estimates
|
||||
void resetGyroBias(void);
|
||||
|
||||
// Resets the baro so that it reads zero at the current height
|
||||
// Resets the EKF height to zero
|
||||
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
|
||||
// Returns true if the height datum reset has been performed
|
||||
// If using a range finder for height no reset is performed and it returns false
|
||||
bool resetHeightDatum(void);
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
// This command must be sent prior to arming as it will only be actioned when the filter is in static mode
|
||||
// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
|
||||
// Returns 0 if command rejected
|
||||
// Returns 1 if attitude, vertical velocity and vertical position will be provided
|
||||
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||||
uint8_t setInhibitGPS(void);
|
||||
|
||||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||||
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
|
||||
|
||||
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getWind(int8_t instance, Vector3f &wind);
|
||||
|
||||
// return earth magnetic field estimates in measurement units / 1000 for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getMagNED(int8_t instance, Vector3f &magNED);
|
||||
|
||||
// return body magnetic field estimates in measurement units / 1000 for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getMagXYZ(int8_t instance, Vector3f &magXYZ);
|
||||
|
||||
// return the magnetometer in use for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
uint8_t getActiveMag(int8_t instance);
|
||||
|
||||
// Return estimated magnetometer offsets
|
||||
// Return true if magnetometer offsets are valid
|
||||
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
|
||||
|
||||
// Return the last calculated latitude, longitude and height in WGS-84
|
||||
// If a calculated location isn't available, return a raw GPS measurement
|
||||
// The status will return true if a calculation or raw measurement is available
|
||||
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
|
||||
bool getLLH(struct Location &loc) const;
|
||||
|
||||
// return the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calculated by the filter are relative to this location
|
||||
// Returns false if the origin has not been set
|
||||
bool getOriginLLH(struct Location &loc) const;
|
||||
|
||||
// set the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calcualted by the filter will be relative to this location
|
||||
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
|
||||
// Returns false if the filter has rejected the attempt to set the origin
|
||||
bool setOriginLLH(struct Location &loc);
|
||||
|
||||
// return estimated height above ground level
|
||||
// return false if ground height is not being estimated.
|
||||
bool getHAGL(float &HAGL) const;
|
||||
|
||||
// return the Euler roll, pitch and yaw angle in radians for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getEulerAngles(int8_t instance, Vector3f &eulers);
|
||||
|
||||
// return the transformation matrix from XYZ (body) to NED axes
|
||||
void getRotationBodyToNED(Matrix3f &mat) const;
|
||||
|
||||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
||||
void getQuaternion(Quaternion &quat) const;
|
||||
|
||||
// return the innovations for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov);
|
||||
|
||||
// publish output observer angular, velocity and position tracking error
|
||||
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
|
||||
|
||||
// return the innovation consistency test ratios for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset);
|
||||
|
||||
// should we use the compass? This is public so it can be used for
|
||||
// reporting via ahrs.use_compass()
|
||||
bool use_compass(void) const;
|
||||
|
||||
// write the raw optical flow measurements
|
||||
// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
|
||||
// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
|
||||
// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
|
||||
// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
|
||||
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
|
||||
// posOffset is the XYZ flow sensor position in the body frame in m
|
||||
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
|
||||
|
||||
// return data for debugging optical flow fusion for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr);
|
||||
|
||||
/*
|
||||
Returns the following data for debugging range beacon fusion from the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
ID : beacon identifier
|
||||
rng : measured range to beacon (m)
|
||||
innov : range innovation (m)
|
||||
innovVar : innovation variance (m^2)
|
||||
testRatio : innovation consistency test ratio
|
||||
beaconPosNED : beacon NED position (m)
|
||||
returns true if data could be found, false if it could not
|
||||
*/
|
||||
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
|
||||
|
||||
// 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 setTakeoffExpected(bool val);
|
||||
|
||||
// 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 setTouchdownExpected(bool val);
|
||||
|
||||
// Set to true if the terrain underneath is stable enough to be used as a height reference
|
||||
// in combination with a range finder. Set to false if the terrain underneath the vehicle
|
||||
// cannot be used as a height reference
|
||||
void setTerrainHgtStable(bool val);
|
||||
|
||||
/*
|
||||
return the filter fault status as a bitmasked integer for the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
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 getFilterFaults(int8_t instance, uint16_t &faults);
|
||||
|
||||
/*
|
||||
return filter timeout status as a bitmasked integer for the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
0 = position measurement timeout
|
||||
1 = velocity measurement timeout
|
||||
2 = height measurement timeout
|
||||
3 = magnetometer measurement timeout
|
||||
5 = unassigned
|
||||
6 = unassigned
|
||||
7 = unassigned
|
||||
7 = unassigned
|
||||
*/
|
||||
void getFilterTimeouts(int8_t instance, uint8_t &timeouts);
|
||||
|
||||
/*
|
||||
return filter gps quality check status for the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
*/
|
||||
void getFilterGpsStatus(int8_t instance, nav_gps_status &faults);
|
||||
|
||||
/*
|
||||
return filter status flags for the specified instance
|
||||
An out of range instance (eg -1) returns data for the the primary instance
|
||||
*/
|
||||
void getFilterStatus(int8_t instance, nav_filter_status &status);
|
||||
|
||||
// send an EKF_STATUS_REPORT message to GCS
|
||||
void send_status_report(mavlink_channel_t chan);
|
||||
|
||||
// provides the height limit to be observed by the control loops
|
||||
// returns false if no height limiting is required
|
||||
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
|
||||
bool getHeightControlLimit(float &height) const;
|
||||
|
||||
// return the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch
|
||||
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
|
||||
uint32_t getLastYawResetAngle(float &yawAngDelta);
|
||||
|
||||
// return the amount of NE position change due to the last position reset in metres
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t getLastPosNorthEastReset(Vector2f &posDelta);
|
||||
|
||||
// return the amount of NE velocity change due to the last velocity reset in metres/sec
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
|
||||
|
||||
// return the amount of vertical position change due to the last reset in metres
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t getLastPosDownReset(float &posDelta);
|
||||
|
||||
// report any reason for why the backend is refusing to initialise
|
||||
const char *prearm_failure_reason(void) const;
|
||||
|
||||
// allow the enable flag to be set by Replay
|
||||
void set_enable(bool enable) { _enable.set(enable); }
|
||||
|
||||
// are we doing sensor logging inside the EKF?
|
||||
bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; }
|
||||
|
||||
private:
|
||||
uint8_t num_cores; // number of allocated cores
|
||||
uint8_t primary; // current primary core
|
||||
NavEKF3_core *core = nullptr;
|
||||
const AP_AHRS *_ahrs;
|
||||
AP_Baro &_baro;
|
||||
const RangeFinder &_rng;
|
||||
|
||||
// EKF Mavlink Tuneable Parameters
|
||||
AP_Int8 _enable; // zero to disable EKF3
|
||||
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
|
||||
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
|
||||
AP_Float _gpsHorizPosNoise; // GPS horizontal position measurement noise m
|
||||
AP_Float _baroAltNoise; // Baro height measurement noise : m
|
||||
AP_Float _magNoise; // magnetometer measurement noise : gauss
|
||||
AP_Float _easNoise; // equivalent airspeed measurement noise : m/s
|
||||
AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2
|
||||
AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate
|
||||
AP_Float _magEarthProcessNoise; // Earth magnetic field process noise : gauss/sec
|
||||
AP_Float _magBodyProcessNoise; // Body magnetic field process noise : gauss/sec
|
||||
AP_Float _gyrNoise; // gyro process noise : rad/s
|
||||
AP_Float _accNoise; // accelerometer process noise : m/s^2
|
||||
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
|
||||
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
|
||||
AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to inertial measurement (msec)
|
||||
AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec)
|
||||
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
|
||||
AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check
|
||||
AP_Int16 _gpsPosInnovGate; // Percentage number of standard deviations applied to GPS position innovation consistency check
|
||||
AP_Int16 _hgtInnovGate; // Percentage number of standard deviations applied to height innovation consistency check
|
||||
AP_Int16 _magInnovGate; // Percentage number of standard deviations applied to magnetometer innovation consistency check
|
||||
AP_Int16 _tasInnovGate; // Percentage number of standard deviations applied to true airspeed innovation consistency check
|
||||
AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration
|
||||
AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m
|
||||
AP_Float _flowNoise; // optical flow rate measurement noise
|
||||
AP_Int16 _flowInnovGate; // Percentage number of standard deviations applied to optical flow innovation consistency check
|
||||
AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
|
||||
AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check
|
||||
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
|
||||
AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
|
||||
AP_Float _rngNoise; // Range finder noise : m
|
||||
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
|
||||
AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for
|
||||
AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
|
||||
AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
|
||||
AP_Int8 _logging_mask; // mask of IMUs to log
|
||||
AP_Float _yawNoise; // magnetic yaw measurement noise : rad
|
||||
AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
|
||||
AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds)
|
||||
AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder in metres
|
||||
AP_Float _terrGradMax; // Maximum terrain gradient below the vehicle
|
||||
AP_Float _rngBcnNoise; // Range beacon measurement noise (m)
|
||||
AP_Int16 _rngBcnInnovGate; // Percentage number of standard deviations applied to range beacon innovation consistency check
|
||||
AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec)
|
||||
|
||||
// Tuning parameters
|
||||
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
|
||||
const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
|
||||
const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
|
||||
const uint16_t magDelay_ms; // Magnetometer measurement delay (msec)
|
||||
const uint16_t tasDelay_ms; // Airspeed measurement delay (msec)
|
||||
const uint16_t tiltDriftTimeMax_ms; // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
|
||||
const uint16_t posRetryTimeUseVel_ms; // Position aiding retry time with velocity measurements (msec)
|
||||
const uint16_t posRetryTimeNoVel_ms; // Position aiding retry time without velocity measurements (msec)
|
||||
const uint16_t hgtRetryTimeMode0_ms; // Height retry time with vertical velocity measurement (msec)
|
||||
const uint16_t hgtRetryTimeMode12_ms; // Height retry time without vertical velocity measurement (msec)
|
||||
const uint16_t tasRetryTime_ms; // True airspeed timeout and retry interval (msec)
|
||||
const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
|
||||
const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
|
||||
const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground
|
||||
const uint16_t hgtAvg_ms; // average number of msec between height measurements
|
||||
const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements
|
||||
const float covTimeStepMax; // maximum time (sec) between covariance prediction updates
|
||||
const float covDelAngMax; // maximum delta angle between covariance prediction updates
|
||||
const float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
|
||||
const float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step
|
||||
const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec)
|
||||
const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events
|
||||
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
|
||||
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
|
||||
const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation
|
||||
const uint8_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec
|
||||
|
||||
struct {
|
||||
bool enabled:1;
|
||||
bool log_compass:1;
|
||||
bool log_gps:1;
|
||||
bool log_baro:1;
|
||||
bool log_imu:1;
|
||||
} logging;
|
||||
|
||||
// time at start of current filter update
|
||||
uint64_t imuSampleTime_us;
|
||||
|
||||
struct {
|
||||
uint32_t last_function_call; // last time getLastYawYawResetAngle was called
|
||||
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise
|
||||
uint32_t last_primary_change; // last time a primary has changed
|
||||
float core_delta; // the amount of yaw change between cores when a change happened
|
||||
} yaw_reset_data;
|
||||
|
||||
struct {
|
||||
uint32_t last_function_call; // last time getLastPosNorthEastReset was called
|
||||
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise
|
||||
uint32_t last_primary_change; // last time a primary has changed
|
||||
Vector2f core_delta; // the amount of NE position change between cores when a change happened
|
||||
} pos_reset_data;
|
||||
|
||||
struct {
|
||||
uint32_t last_function_call; // last time getLastPosDownReset was called
|
||||
bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise
|
||||
uint32_t last_primary_change; // last time a primary has changed
|
||||
float core_delta; // the amount of D position change between cores when a change happened
|
||||
} pos_down_reset_data;
|
||||
|
||||
// update the yaw reset data to capture changes due to a lane switch
|
||||
// new_primary - index of the ekf instance that we are about to switch to as the primary
|
||||
// old_primary - index of the ekf instance that we are currently using as the primary
|
||||
void updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary);
|
||||
|
||||
// update the position reset data to capture changes due to a lane switch
|
||||
// new_primary - index of the ekf instance that we are about to switch to as the primary
|
||||
// old_primary - index of the ekf instance that we are currently using as the primary
|
||||
void updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary);
|
||||
|
||||
// update the position down reset data to capture changes due to a lane switch
|
||||
// new_primary - index of the ekf instance that we are about to switch to as the primary
|
||||
// old_primary - index of the ekf instance that we are currently using as the primary
|
||||
void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary);
|
||||
};
|
||||
440
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
Normal file
440
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
Normal file
File diff suppressed because one or more lines are too long
188
libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h
Normal file
188
libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h
Normal file
@ -0,0 +1,188 @@
|
||||
// EKF Buffer models
|
||||
|
||||
// this buffer model is to be used for observation buffers,
|
||||
// the data is pushed into buffer like any standard ring buffer
|
||||
// return is based on the sample time provided
|
||||
template <typename element_type>
|
||||
class obs_ring_buffer_t
|
||||
{
|
||||
public:
|
||||
struct element_t{
|
||||
element_type element;
|
||||
} *buffer;
|
||||
|
||||
// initialise buffer, returns false when allocation has failed
|
||||
bool init(uint32_t size)
|
||||
{
|
||||
buffer = new element_t[size];
|
||||
if(buffer == nullptr)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(buffer,0,size*sizeof(element_t));
|
||||
_size = size;
|
||||
_head = 0;
|
||||
_tail = 0;
|
||||
_new_data = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Searches through a ring buffer and return the newest data that is older than the
|
||||
* time specified by sample_time_ms
|
||||
* Zeros old data so it cannot not be used again
|
||||
* Returns false if no data can be found that is less than 100msec old
|
||||
*/
|
||||
|
||||
bool recall(element_type &element,uint32_t sample_time)
|
||||
{
|
||||
if(!_new_data) {
|
||||
return false;
|
||||
}
|
||||
bool success = false;
|
||||
uint8_t tail = _tail, bestIndex;
|
||||
|
||||
if(_head == tail) {
|
||||
if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
|
||||
// if head is equal to tail just check if the data is unused and within time horizon window
|
||||
if (((sample_time - buffer[tail].element.time_ms) < 100)) {
|
||||
bestIndex = tail;
|
||||
success = true;
|
||||
_new_data = false;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
while(_head != tail) {
|
||||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||||
if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
|
||||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||||
if (((sample_time - buffer[tail].element.time_ms) < 100)) {
|
||||
bestIndex = tail;
|
||||
success = true;
|
||||
}
|
||||
} else if(buffer[tail].element.time_ms > sample_time){
|
||||
break;
|
||||
}
|
||||
tail = (tail+1)%_size;
|
||||
}
|
||||
}
|
||||
|
||||
if (success) {
|
||||
element = buffer[bestIndex].element;
|
||||
_tail = (bestIndex+1)%_size;
|
||||
//make time zero to stop using it again,
|
||||
//resolves corner case of reusing the element when head == tail
|
||||
buffer[bestIndex].element.time_ms = 0;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Writes data and timestamp to a Ring buffer and advances indices that
|
||||
* define the location of the newest and oldest data
|
||||
*/
|
||||
inline void push(element_type element)
|
||||
{
|
||||
// Advance head to next available index
|
||||
_head = (_head+1)%_size;
|
||||
// New data is written at the head
|
||||
buffer[_head].element = element;
|
||||
_new_data = true;
|
||||
}
|
||||
// writes the same data to all elements in the ring buffer
|
||||
inline void reset_history(element_type element, uint32_t sample_time) {
|
||||
for (uint8_t index=0; index<_size; index++) {
|
||||
buffer[index].element = element;
|
||||
}
|
||||
}
|
||||
|
||||
// zeroes all data in the ring buffer
|
||||
inline void reset() {
|
||||
_head = 0;
|
||||
_tail = 0;
|
||||
_new_data = false;
|
||||
memset(buffer,0,_size*sizeof(element_t));
|
||||
}
|
||||
|
||||
private:
|
||||
uint8_t _size,_head,_tail,_new_data;
|
||||
};
|
||||
|
||||
|
||||
// Following buffer model is for IMU data,
|
||||
// it achieves a distance of sample size
|
||||
// between youngest and oldest
|
||||
template <typename element_type>
|
||||
class imu_ring_buffer_t
|
||||
{
|
||||
public:
|
||||
struct element_t{
|
||||
element_type element;
|
||||
} *buffer;
|
||||
|
||||
// initialise buffer, returns false when allocation has failed
|
||||
bool init(uint32_t size)
|
||||
{
|
||||
buffer = new element_t[size];
|
||||
if(buffer == nullptr)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(buffer,0,size*sizeof(element_t));
|
||||
_size = size;
|
||||
_youngest = 0;
|
||||
_oldest = 0;
|
||||
return true;
|
||||
}
|
||||
/*
|
||||
* Writes data to a Ring buffer and advances indices that
|
||||
* define the location of the newest and oldest data
|
||||
*/
|
||||
inline void push_youngest_element(element_type element)
|
||||
{
|
||||
// push youngest to the buffer
|
||||
_youngest = (_youngest+1)%_size;
|
||||
buffer[_youngest].element = element;
|
||||
// set oldest data index
|
||||
_oldest = (_youngest+1)%_size;
|
||||
}
|
||||
|
||||
// retrieve the oldest data from the ring buffer tail
|
||||
inline element_type pop_oldest_element() {
|
||||
element_type ret = buffer[_oldest].element;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// writes the same data to all elements in the ring buffer
|
||||
inline void reset_history(element_type element) {
|
||||
for (uint8_t index=0; index<_size; index++) {
|
||||
buffer[index].element = element;
|
||||
}
|
||||
}
|
||||
|
||||
// zeroes all data in the ring buffer
|
||||
inline void reset() {
|
||||
_youngest = 0;
|
||||
_oldest = 0;
|
||||
memset(buffer,0,_size*sizeof(element_t));
|
||||
}
|
||||
|
||||
// retrieves data from the ring buffer at a specified index
|
||||
inline element_type& operator[](uint32_t index) {
|
||||
return buffer[index].element;
|
||||
}
|
||||
|
||||
// returns the index for the ring buffer oldest data
|
||||
inline uint8_t get_oldest_index(){
|
||||
return _oldest;
|
||||
}
|
||||
|
||||
// returns the index for the ring buffer youngest data
|
||||
inline uint8_t get_youngest_index(){
|
||||
return _youngest;
|
||||
}
|
||||
private:
|
||||
uint8_t _size,_oldest,_youngest;
|
||||
};
|
||||
502
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Normal file
502
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Normal file
@ -0,0 +1,502 @@
|
||||
/// -*- 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
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
// Control filter mode transitions
|
||||
void NavEKF3_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();
|
||||
|
||||
// Set the type of inertial navigation aiding used
|
||||
setAidingMode();
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
return effective value for _magCal for this core
|
||||
*/
|
||||
uint8_t NavEKF3_core::effective_magCal(void) const
|
||||
{
|
||||
// if we are on the 2nd core and _magCal is 3 then treat it as
|
||||
// 2. This is a workaround for a mag fusion problem
|
||||
if (frontend->_magCal ==3 && imu_index == 1) {
|
||||
return 2;
|
||||
}
|
||||
return frontend->_magCal;
|
||||
}
|
||||
|
||||
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
|
||||
// avoid unnecessary operations
|
||||
void NavEKF3_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
|
||||
bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
|
||||
if (!inhibitWindStates && setWindInhibit) {
|
||||
inhibitWindStates = true;
|
||||
} else if (inhibitWindStates && !setWindInhibit) {
|
||||
inhibitWindStates = false;
|
||||
// set states and variances
|
||||
if (yawAlignComplete && useAirspeed()) {
|
||||
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
|
||||
// which assumes the vehicle has launched into the wind
|
||||
Vector3f tempEuler;
|
||||
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
|
||||
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
|
||||
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
|
||||
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
|
||||
|
||||
// set the wind sate variances to the measurement uncertainty
|
||||
for (uint8_t index=22; index<=23; index++) {
|
||||
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
|
||||
}
|
||||
} else {
|
||||
// set the variances using a typical wind speed
|
||||
for (uint8_t index=22; index<=23; index++) {
|
||||
P[index][index] = sq(5.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
uint8_t magCal = effective_magCal();
|
||||
bool magCalRequested =
|
||||
((magCal == 0) && inFlight) || // when flying
|
||||
((magCal == 1) && manoeuvring) || // when manoeuvring
|
||||
((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
|
||||
(magCal == 4); // all the time
|
||||
|
||||
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
|
||||
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
|
||||
bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
|
||||
|
||||
// Inhibit the magnetic field calibration if not requested or denied
|
||||
bool setMagInhibit = !magCalRequested || magCalDenied;
|
||||
if (!inhibitMagStates && setMagInhibit) {
|
||||
inhibitMagStates = true;
|
||||
} else if (inhibitMagStates && !setMagInhibit) {
|
||||
inhibitMagStates = false;
|
||||
if (magFieldLearned) {
|
||||
// if we have already learned the field states, then retain the learned variances
|
||||
P[16][16] = earthMagFieldVar.x;
|
||||
P[17][17] = earthMagFieldVar.y;
|
||||
P[18][18] = earthMagFieldVar.z;
|
||||
P[19][19] = bodyMagFieldVar.x;
|
||||
P[20][20] = bodyMagFieldVar.y;
|
||||
P[21][21] = bodyMagFieldVar.z;
|
||||
} else {
|
||||
// set the variances equal to the observation variances
|
||||
for (uint8_t index=18; index<=21; index++) {
|
||||
P[index][index] = sq(frontend->_magNoise);
|
||||
}
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
}
|
||||
// request a reset of the yaw and magnetic field states if not done before
|
||||
if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
|
||||
magYawResetRequest = true;
|
||||
}
|
||||
}
|
||||
|
||||
// inhibit delta velocity bias learning if we have not yet aligned the tilt
|
||||
if (tiltAlignComplete && inhibitDelVelBiasStates) {
|
||||
// activate the states
|
||||
inhibitDelVelBiasStates = false;
|
||||
// set the initial covariance values
|
||||
P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
|
||||
P[14][14] = P[13][13];
|
||||
P[15][15] = P[13][13];
|
||||
}
|
||||
|
||||
if (tiltAlignComplete && inhibitDelAngBiasStates) {
|
||||
// activate the states
|
||||
inhibitDelAngBiasStates = false;
|
||||
// set the initial covariance values
|
||||
P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
|
||||
P[11][11] = P[10][10];
|
||||
P[12][12] = P[10][10];
|
||||
}
|
||||
|
||||
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
|
||||
// because we want it re-done for each takeoff
|
||||
if (onGround) {
|
||||
finalInflightYawInit = false;
|
||||
finalInflightMagInit = false;
|
||||
}
|
||||
|
||||
// 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 && inhibitDelVelBiasStates) {
|
||||
stateIndexLim = 12;
|
||||
} else if (inhibitMagStates && !inhibitWindStates) {
|
||||
stateIndexLim = 15;
|
||||
} else if (inhibitWindStates) {
|
||||
stateIndexLim = 21;
|
||||
} else {
|
||||
stateIndexLim = 23;
|
||||
}
|
||||
}
|
||||
|
||||
// Set inertial navigation aiding mode
|
||||
void NavEKF3_core::setAidingMode()
|
||||
{
|
||||
// Save the previous status so we can detect when it has changed
|
||||
PV_AidingModePrev = PV_AidingMode;
|
||||
|
||||
// Determine if we should change aiding mode
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
|
||||
// and IMU gyro bias estimates have stabilised
|
||||
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
|
||||
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
|
||||
// GPS aiding is the preferred option unless excluded by the user
|
||||
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
|
||||
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
|
||||
if(canUseGPS || canUseRangeBeacon) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
} else if (optFlowDataPresent() && filterIsStable) {
|
||||
PV_AidingMode = AID_RELATIVE;
|
||||
}
|
||||
} else if (PV_AidingMode == AID_RELATIVE) {
|
||||
// Check if the optical flow sensor has timed out
|
||||
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
|
||||
// Check if the fusion has timed out (flow measurements have been rejected for too long)
|
||||
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
|
||||
// Enable switch to absolute position mode if GPS is available
|
||||
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
|
||||
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
|
||||
PV_AidingMode = AID_ABSOLUTE;
|
||||
} else if (flowSensorTimeout || flowFusionTimeout) {
|
||||
PV_AidingMode = AID_NONE;
|
||||
}
|
||||
} else if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
// Find the minimum time without data required to trigger any check
|
||||
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
|
||||
|
||||
// Check if optical flow data is being used
|
||||
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms > minTestTime_ms);
|
||||
|
||||
// Check if airspeed data is being used
|
||||
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms > minTestTime_ms);
|
||||
|
||||
// Check if range beacon data is being used
|
||||
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms > minTestTime_ms);
|
||||
|
||||
// Check if GPS is being used
|
||||
bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms > minTestTime_ms);
|
||||
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms > minTestTime_ms);
|
||||
|
||||
// Check if attitude drift has been constrained by a measurement source
|
||||
bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
|
||||
|
||||
// check if velocity drift has been constrained by a measurement source
|
||||
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
|
||||
|
||||
// check if position drift has been constrained by a measurement source
|
||||
bool posAiding = gpsPosUsed || rngBcnUsed;
|
||||
|
||||
// Check if the loss of attitude aiding has become critical
|
||||
bool attAidLossCritical = false;
|
||||
if (!attAiding) {
|
||||
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
|
||||
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
|
||||
}
|
||||
|
||||
// Check if the loss of position accuracy has become critical
|
||||
bool posAidLossCritical = false;
|
||||
if (!posAiding ) {
|
||||
uint16_t maxLossTime_ms;
|
||||
if (!velAiding) {
|
||||
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
|
||||
} else {
|
||||
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
|
||||
}
|
||||
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
|
||||
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms) &&
|
||||
(imuSampleTime_ms - lastVelPassTime_ms > maxLossTime_ms);
|
||||
}
|
||||
|
||||
if (attAidLossCritical) {
|
||||
// if the loss of attitude data is critical, then put the filter into a constant position mode
|
||||
PV_AidingMode = AID_NONE;
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
rngBcnTimeout = true;
|
||||
tasTimeout = true;
|
||||
gpsNotAvailable = true;
|
||||
} else if (posAidLossCritical) {
|
||||
// if the loss of position is critical, declare all sources of position aiding as being timed out
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
rngBcnTimeout = true;
|
||||
gpsNotAvailable = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// check to see if we are starting or stopping aiding and set states and modes as required
|
||||
if (PV_AidingMode != PV_AidingModePrev) {
|
||||
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
// We have ceased aiding
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u has stopped aiding",(unsigned)imu_index);
|
||||
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
|
||||
posTimeout = true;
|
||||
velTimeout = true;
|
||||
// Reset the normalised innovation to avoid false failing bad fusion tests
|
||||
velTestRatio = 0.0f;
|
||||
posTestRatio = 0.0f;
|
||||
// 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 (PV_AidingMode == AID_RELATIVE) {
|
||||
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using optical flow",(unsigned)imu_index);
|
||||
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 if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
|
||||
bool canUseRangeBeacon = readyToUseRangeBeacon();
|
||||
// We have commenced aiding and GPS usage is allowed
|
||||
if (canUseGPS) {
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
|
||||
}
|
||||
posTimeout = false;
|
||||
velTimeout = false;
|
||||
// We have commenced aiding and range beacon usage is allowed
|
||||
if (canUseRangeBeacon) {
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
|
||||
}
|
||||
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// Always reset the position and velocity when changing mode
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Check the tilt and yaw alignmnent status
|
||||
// Used during initial bootstrap alignment of the filter
|
||||
void NavEKF3_core::checkAttitudeAlignmentStatus()
|
||||
{
|
||||
// Check for tilt convergence - used during initial alignment
|
||||
if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.03f) && !tiltAlignComplete) {
|
||||
tiltAlignComplete = true;
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// submit yaw and magnetic field reset requests depending on whether we have compass data
|
||||
if (tiltAlignComplete && !yawAlignComplete) {
|
||||
if (use_compass()) {
|
||||
magYawResetRequest = true;
|
||||
gpsYawResetRequest = false;
|
||||
} else {
|
||||
magYawResetRequest = false;
|
||||
gpsYawResetRequest = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return true if we should use the airspeed sensor
|
||||
bool NavEKF3_core::useAirspeed(void) const
|
||||
{
|
||||
return _ahrs->airspeed_sensor_enabled();
|
||||
}
|
||||
|
||||
// return true if we should use the range finder sensor
|
||||
bool NavEKF3_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 NavEKF3_core::optFlowDataPresent(void) const
|
||||
{
|
||||
return (imuSampleTime_ms - flowMeaTime_ms < 200);
|
||||
}
|
||||
|
||||
// return true if the filter to be ready to use gps
|
||||
bool NavEKF3_core::readyToUseGPS(void) const
|
||||
{
|
||||
return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse;
|
||||
}
|
||||
|
||||
// return true if the filter to be ready to use the beacon range measurements
|
||||
bool NavEKF3_core::readyToUseRangeBeacon(void) const
|
||||
{
|
||||
return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse;
|
||||
}
|
||||
|
||||
// return true if we should use the compass
|
||||
bool NavEKF3_core::use_compass(void) const
|
||||
{
|
||||
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
|
||||
}
|
||||
|
||||
/*
|
||||
should we assume zero sideslip?
|
||||
*/
|
||||
bool NavEKF3_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;
|
||||
}
|
||||
|
||||
// set the LLH location of the filters NED origin
|
||||
bool NavEKF3_core::setOriginLLH(struct Location &loc)
|
||||
{
|
||||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
return false;
|
||||
}
|
||||
EKF_origin = loc;
|
||||
// define Earth rotation vector in the NED navigation frame at the origin
|
||||
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
|
||||
validOrigin = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Set the NED origin to be used until the next filter reset
|
||||
void NavEKF3_core::setOrigin()
|
||||
{
|
||||
// assume origin at current GPS location (no averaging)
|
||||
EKF_origin = _ahrs->get_gps().location();
|
||||
// define Earth rotation vector in the NED navigation frame at the origin
|
||||
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
|
||||
validOrigin = true;
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index);
|
||||
}
|
||||
|
||||
// record a yaw reset event
|
||||
void NavEKF3_core::recordYawReset()
|
||||
{
|
||||
yawAlignComplete = true;
|
||||
if (inFlight) {
|
||||
finalInflightYawInit = true;
|
||||
}
|
||||
}
|
||||
|
||||
// return true and set the class variable true if the delta angle bias has been learned
|
||||
bool NavEKF3_core::checkGyroCalStatus(void)
|
||||
{
|
||||
// check delta angle bias variances
|
||||
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
|
||||
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
|
||||
(P[11][11] <= delAngBiasVarMax) &&
|
||||
(P[12][12] <= delAngBiasVarMax);
|
||||
return delAngBiasLearned;
|
||||
}
|
||||
|
||||
// Commands the EKF to not use GPS.
|
||||
// This command must be sent prior to arming
|
||||
// This command is forgotten by the EKF each time the vehicle disarms
|
||||
// Returns 0 if command rejected
|
||||
// Returns 1 if attitude, vertical velocity and vertical position will be provided
|
||||
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
|
||||
uint8_t NavEKF3_core::setInhibitGPS(void)
|
||||
{
|
||||
if((PV_AidingMode == AID_ABSOLUTE) && motorsArmed) {
|
||||
return 0;
|
||||
} else {
|
||||
gpsInhibit = true;
|
||||
return 1;
|
||||
}
|
||||
// option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation
|
||||
}
|
||||
|
||||
// Update the filter status
|
||||
void NavEKF3_core::updateFilterStatus(void)
|
||||
{
|
||||
// init return value
|
||||
filterStatus.value = 0;
|
||||
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
|
||||
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
|
||||
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
|
||||
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
|
||||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
|
||||
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
|
||||
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
|
||||
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
|
||||
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
|
||||
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
|
||||
|
||||
// set individual flags
|
||||
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
|
||||
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
|
||||
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
|
||||
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
|
||||
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
|
||||
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
|
||||
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
|
||||
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
|
||||
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
|
||||
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
|
||||
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
|
||||
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
|
||||
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
|
||||
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
|
||||
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
37
libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp
Normal file
37
libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp
Normal file
@ -0,0 +1,37 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
|
||||
// Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions
|
||||
// WARNING - a non-blocking calibration method must be used
|
||||
void NavEKF3_core::resetGyroBias(void)
|
||||
{
|
||||
stateStruct.gyro_bias.zero();
|
||||
zeroRows(P,10,12);
|
||||
zeroCols(P,10,12);
|
||||
|
||||
P[10][10] = sq(radians(0.5f * dtIMUavg));
|
||||
P[11][11] = P[10][10];
|
||||
P[12][12] = P[10][10];
|
||||
}
|
||||
|
||||
/*
|
||||
vehicle specific initial gyro bias uncertainty in deg/sec
|
||||
*/
|
||||
float NavEKF3_core::InitialGyroBiasUncertainty(void) const
|
||||
{
|
||||
return 2.5f;
|
||||
}
|
||||
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
1133
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
Normal file
1133
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
Normal file
File diff suppressed because it is too large
Load Diff
760
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Normal file
760
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Normal file
@ -0,0 +1,760 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
/********************************************************
|
||||
* OPT FLOW AND RANGE FINDER *
|
||||
********************************************************/
|
||||
|
||||
// Read the range finder and take new measurements if available
|
||||
// Apply a median filter
|
||||
void NavEKF3_core::readRangeFinder(void)
|
||||
{
|
||||
uint8_t midIndex;
|
||||
uint8_t maxIndex;
|
||||
uint8_t minIndex;
|
||||
// get theoretical correct range when the vehicle is on the ground
|
||||
rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f;
|
||||
|
||||
// read range finder at 20Hz
|
||||
// TODO better way of knowing if it has new data
|
||||
if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||||
|
||||
// reset the timer used to control the measurement rate
|
||||
lastRngMeasTime_ms = imuSampleTime_ms;
|
||||
|
||||
// store samples and sample time into a ring buffer if valid
|
||||
// use data from two range finders if available
|
||||
|
||||
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
|
||||
if (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good) {
|
||||
rngMeasIndex[sensorIndex] ++;
|
||||
if (rngMeasIndex[sensorIndex] > 2) {
|
||||
rngMeasIndex[sensorIndex] = 0;
|
||||
}
|
||||
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
|
||||
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f;
|
||||
}
|
||||
|
||||
// check for three fresh samples
|
||||
bool sampleFresh[2][3] = {};
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
|
||||
}
|
||||
|
||||
// find the median value if we have three fresh samples
|
||||
if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
|
||||
if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
|
||||
minIndex = 1;
|
||||
maxIndex = 0;
|
||||
} else {
|
||||
minIndex = 0;
|
||||
maxIndex = 1;
|
||||
}
|
||||
if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
|
||||
midIndex = maxIndex;
|
||||
} else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
|
||||
midIndex = minIndex;
|
||||
} else {
|
||||
midIndex = 2;
|
||||
}
|
||||
|
||||
// don't allow time to go backwards
|
||||
if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
|
||||
rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
|
||||
}
|
||||
|
||||
// limit the measured range to be no less than the on-ground range
|
||||
rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
|
||||
|
||||
// get position in body frame for the current sensor
|
||||
rangeDataNew.sensor_idx = sensorIndex;
|
||||
|
||||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||||
storedRange.push(rangeDataNew);
|
||||
|
||||
// indicate we have updated the measurement
|
||||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||||
|
||||
} else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
|
||||
// before takeoff we assume on-ground range value if there is no data
|
||||
rangeDataNew.time_ms = imuSampleTime_ms;
|
||||
rangeDataNew.rng = rngOnGnd;
|
||||
rangeDataNew.time_ms = imuSampleTime_ms;
|
||||
|
||||
// don't allow time to go backwards
|
||||
if (imuSampleTime_ms > rangeDataNew.time_ms) {
|
||||
rangeDataNew.time_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||||
storedRange.push(rangeDataNew);
|
||||
|
||||
// indicate we have updated the measurement
|
||||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// write the raw optical flow measurements
|
||||
// this needs to be called externally.
|
||||
void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
|
||||
{
|
||||
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
|
||||
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
|
||||
// A positive X rate is produced by a positive sensor rotation about the X axis
|
||||
// A positive Y rate is produced by a positive sensor rotation about the Y axis
|
||||
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
|
||||
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
|
||||
flowMeaTime_ms = imuSampleTime_ms;
|
||||
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
|
||||
// reset the accumulated body delta angle and time
|
||||
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
|
||||
if (delTimeOF > 0.01f) {
|
||||
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
|
||||
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
|
||||
delAngBodyOF.zero();
|
||||
delTimeOF = 0.0f;
|
||||
}
|
||||
// by definition if this function is called, then flow measurements have been provided so we
|
||||
// need to run the optical flow takeoff detection
|
||||
detectOptFlowTakeoff();
|
||||
|
||||
// calculate rotation matrices at mid sample time for flow observations
|
||||
stateStruct.quat.rotation_matrix(Tbn_flow);
|
||||
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
|
||||
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
|
||||
// correct flow sensor body rates for bias and write
|
||||
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
|
||||
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
|
||||
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
|
||||
if (delTimeOF > 0.001f) {
|
||||
// first preference is to use the rate averaged over the same sampling period as the flow sensor
|
||||
ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
|
||||
} else if (imuDataNew.delAngDT > 0.001f){
|
||||
// second preference is to use most recent IMU data
|
||||
ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
|
||||
} else {
|
||||
// third preference is use zero
|
||||
ofDataNew.bodyRadXYZ.z = 0.0f;
|
||||
}
|
||||
// write uncorrected flow rate measurements
|
||||
// note correction for different axis and sign conventions used by the px4flow sensor
|
||||
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
|
||||
// write the flow sensor position in body frame
|
||||
ofDataNew.body_offset = &posOffset;
|
||||
// write flow rate measurements corrected for body rates
|
||||
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
|
||||
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
|
||||
// record time last observation was received so we can detect loss of data elsewhere
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
// estimate sample time of the measurement
|
||||
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
// Save data to buffer
|
||||
storedOF.push(ofDataNew);
|
||||
// Check for data at the fusion time horizon
|
||||
flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/********************************************************
|
||||
* MAGNETOMETER *
|
||||
********************************************************/
|
||||
|
||||
// check for new magnetometer data and update store measurements if available
|
||||
void NavEKF3_core::readMagData()
|
||||
{
|
||||
if (!_ahrs->get_compass()) {
|
||||
allMagSensorsFailed = true;
|
||||
return;
|
||||
}
|
||||
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
|
||||
// magnetometer, then declare the magnetometers as failed for this flight
|
||||
uint8_t maxCount = _ahrs->get_compass()->get_count();
|
||||
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
|
||||
allMagSensorsFailed = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
|
||||
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
|
||||
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
|
||||
frontend->logging.log_compass = true;
|
||||
|
||||
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
|
||||
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
|
||||
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
|
||||
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
|
||||
|
||||
// search through the list of magnetometers
|
||||
for (uint8_t i=1; i<maxCount; i++) {
|
||||
uint8_t tempIndex = magSelectIndex + i;
|
||||
// loop back to the start index if we have exceeded the bounds
|
||||
if (tempIndex >= maxCount) {
|
||||
tempIndex -= maxCount;
|
||||
}
|
||||
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
|
||||
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
|
||||
magSelectIndex = tempIndex;
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
|
||||
// reset the timeout flag and timer
|
||||
magTimeout = false;
|
||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||
// zero the learned magnetometer bias states
|
||||
stateStruct.body_magfield.zero();
|
||||
// clear the measurement buffer
|
||||
storedMag.reset();
|
||||
// clear the data waiting flag so that we do not use any data pending from the previous sensor
|
||||
magDataToFuse = false;
|
||||
// request a reset of the magnetic field states
|
||||
magStateResetRequest = true;
|
||||
// declare the field unlearned so that the reset request will be obeyed
|
||||
magFieldLearned = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// detect changes to magnetometer offset parameters and reset states
|
||||
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
|
||||
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
|
||||
if (changeDetected) {
|
||||
// zero the learned magnetometer bias states
|
||||
stateStruct.body_magfield.zero();
|
||||
// clear the measurement buffer
|
||||
storedMag.reset();
|
||||
}
|
||||
lastMagOffsets = nowMagOffsets;
|
||||
lastMagOffsetsValid = true;
|
||||
|
||||
// store time of last measurement update
|
||||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
|
||||
|
||||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||||
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
magDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// read compass data and scale to improve numerical conditioning
|
||||
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
|
||||
|
||||
// check for consistent data between magnetometers
|
||||
consistentMagData = _ahrs->get_compass()->consistent();
|
||||
|
||||
// save magnetometer measurement to buffer to be fused later
|
||||
storedMag.push(magDataNew);
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* Inertial Measurements *
|
||||
********************************************************/
|
||||
|
||||
/*
|
||||
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
|
||||
* for storage in the data buffers used by the EKF. If the IMU data arrives at
|
||||
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
|
||||
* Downsampling is done using a method that does not introduce coning or sculling
|
||||
* errors.
|
||||
*/
|
||||
void NavEKF3_core::readIMUData()
|
||||
{
|
||||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||||
|
||||
// average IMU sampling rate
|
||||
dtIMUavg = ins.get_loop_delta_t();
|
||||
|
||||
// the imu sample time is used as a common time reference throughout the filter
|
||||
imuSampleTime_ms = AP_HAL::millis();
|
||||
|
||||
// use the nominated imu or primary if not available
|
||||
if (ins.use_accel(imu_index)) {
|
||||
readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
|
||||
accelPosOffset = ins.get_imu_pos_offset(imu_index);
|
||||
} else {
|
||||
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
|
||||
accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel());
|
||||
}
|
||||
|
||||
// Get delta angle data from primary gyro or primary if not available
|
||||
if (ins.use_gyro(imu_index)) {
|
||||
readDeltaAngle(imu_index, imuDataNew.delAng);
|
||||
} else {
|
||||
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
|
||||
}
|
||||
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f);
|
||||
|
||||
// Get current time stamp
|
||||
imuDataNew.time_ms = imuSampleTime_ms;
|
||||
|
||||
// Accumulate the measurement time interval for the delta velocity and angle data
|
||||
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
|
||||
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
|
||||
|
||||
// Rotate quaternon atitude from previous to new and normalise.
|
||||
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
|
||||
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
|
||||
imuQuatDownSampleNew.normalize();
|
||||
|
||||
// Rotate the latest delta velocity into body frame at the start of accumulation
|
||||
Matrix3f deltaRotMat;
|
||||
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
|
||||
|
||||
// Apply the delta velocity to the delta velocity accumulator
|
||||
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
|
||||
|
||||
// Keep track of the number of IMU frames since the last state prediction
|
||||
framesSincePredict++;
|
||||
|
||||
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
|
||||
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
|
||||
if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
|
||||
|
||||
// convert the accumulated quaternion to an equivalent delta angle
|
||||
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
|
||||
|
||||
// Time stamp the data
|
||||
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
|
||||
|
||||
// Write data to the FIFO IMU buffer
|
||||
storedIMU.push_youngest_element(imuDataDownSampledNew);
|
||||
|
||||
// calculate the achieved average time step rate for the EKF
|
||||
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT);
|
||||
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
|
||||
|
||||
// zero the accumulated IMU data and quaternion
|
||||
imuDataDownSampledNew.delAng.zero();
|
||||
imuDataDownSampledNew.delVel.zero();
|
||||
imuDataDownSampledNew.delAngDT = 0.0f;
|
||||
imuDataDownSampledNew.delVelDT = 0.0f;
|
||||
imuQuatDownSampleNew[0] = 1.0f;
|
||||
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
|
||||
|
||||
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
|
||||
framesSincePredict = 0;
|
||||
|
||||
// set the flag to let the filter know it has new IMU data nad needs to run
|
||||
runUpdates = true;
|
||||
|
||||
// extract the oldest available data from the FIFO buffer
|
||||
imuDataDelayed = storedIMU.pop_oldest_element();
|
||||
|
||||
// protect against delta time going to zero
|
||||
// TODO - check if calculations can tolerate 0
|
||||
float minDT = 0.1f*dtEkfAvg;
|
||||
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
|
||||
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
|
||||
|
||||
// correct the extracted IMU data for sensor errors
|
||||
delAngCorrected = imuDataDelayed.delAng;
|
||||
delVelCorrected = imuDataDelayed.delVel;
|
||||
correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT);
|
||||
correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT);
|
||||
|
||||
} else {
|
||||
// we don't have new IMU data in the buffer so don't run filter updates on this time step
|
||||
runUpdates = false;
|
||||
}
|
||||
}
|
||||
|
||||
// read the delta velocity and corresponding time interval from the IMU
|
||||
// return false if data is not available
|
||||
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
|
||||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||||
|
||||
if (ins_index < ins.get_accel_count()) {
|
||||
ins.get_delta_velocity(ins_index,dVel);
|
||||
dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* Global Position Measurement *
|
||||
********************************************************/
|
||||
|
||||
// check for new valid GPS data and update stored measurement if available
|
||||
void NavEKF3_core::readGpsData()
|
||||
{
|
||||
// check for new GPS data
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
|
||||
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
// report GPS fix status
|
||||
gpsCheckStatus.bad_fix = false;
|
||||
|
||||
// store fix time from previous read
|
||||
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
|
||||
|
||||
// get current fix time
|
||||
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
|
||||
|
||||
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
|
||||
// ideally we should be using a timing signal from the GPS receiver to set this time
|
||||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
|
||||
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
|
||||
// Get which GPS we are using for position information
|
||||
gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor();
|
||||
|
||||
// read the NED velocity from the GPS
|
||||
gpsDataNew.vel = _ahrs->get_gps().velocity();
|
||||
|
||||
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
|
||||
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
|
||||
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
|
||||
gpsSpdAccuracy *= (1.0f - alpha);
|
||||
float gpsSpdAccRaw;
|
||||
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
|
||||
gpsSpdAccuracy = 0.0f;
|
||||
} else {
|
||||
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
|
||||
gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
|
||||
}
|
||||
gpsPosAccuracy *= (1.0f - alpha);
|
||||
float gpsPosAccRaw;
|
||||
if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) {
|
||||
gpsPosAccuracy = 0.0f;
|
||||
} else {
|
||||
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
|
||||
gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
|
||||
}
|
||||
gpsHgtAccuracy *= (1.0f - alpha);
|
||||
float gpsHgtAccRaw;
|
||||
if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) {
|
||||
gpsHgtAccuracy = 0.0f;
|
||||
} else {
|
||||
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
|
||||
gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
|
||||
}
|
||||
|
||||
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
|
||||
if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
|
||||
gpsNoiseScaler = 1.0f;
|
||||
} else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
|
||||
gpsNoiseScaler = 1.4f;
|
||||
} else { // <= 4 satellites or in constant position mode
|
||||
gpsNoiseScaler = 2.0f;
|
||||
}
|
||||
|
||||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
|
||||
useGpsVertVel = true;
|
||||
} else {
|
||||
useGpsVertVel = false;
|
||||
}
|
||||
|
||||
// Monitor quality of the GPS velocity data before and after alignment using separate checks
|
||||
if (PV_AidingMode != AID_ABSOLUTE) {
|
||||
// Pre-alignment checks
|
||||
gpsGoodToAlign = calcGpsGoodToAlign();
|
||||
} else {
|
||||
gpsGoodToAlign = false;
|
||||
}
|
||||
|
||||
// Post-alignment checks
|
||||
calcGpsGoodForFlight();
|
||||
|
||||
// Read the GPS locaton in WGS-84 lat,long,height coordinates
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
|
||||
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
|
||||
if (gpsGoodToAlign && !validOrigin) {
|
||||
setOrigin();
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
|
||||
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
|
||||
|
||||
// Set the uncertinty of the GPS origin height
|
||||
ekfOriginHgtVar = sq(gpsHgtAccuracy);
|
||||
|
||||
}
|
||||
|
||||
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
|
||||
if (validOrigin) {
|
||||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||||
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
|
||||
storedGPS.push(gpsDataNew);
|
||||
// declare GPS available for use
|
||||
gpsNotAvailable = false;
|
||||
}
|
||||
|
||||
frontend->logging.log_gps = true;
|
||||
|
||||
} else {
|
||||
// report GPS fix status
|
||||
gpsCheckStatus.bad_fix = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// read the delta angle and corresponding time interval from the IMU
|
||||
// return false if data is not available
|
||||
bool NavEKF3_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);
|
||||
frontend->logging.log_imu = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
/********************************************************
|
||||
* Height Measurements *
|
||||
********************************************************/
|
||||
|
||||
// check for new pressure altitude measurement data and update stored measurement if available
|
||||
void NavEKF3_core::readBaroData()
|
||||
{
|
||||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) {
|
||||
frontend->logging.log_baro = true;
|
||||
|
||||
baroDataNew.hgt = frontend->_baro.get_altitude();
|
||||
|
||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
||||
if (getTakeoffExpected()) {
|
||||
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
|
||||
}
|
||||
|
||||
// time stamp used to check for new measurement
|
||||
lastBaroReceived_ms = frontend->_baro.get_last_update();
|
||||
|
||||
// estimate of time height measurement was taken, allowing for delays
|
||||
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
|
||||
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
baroDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
|
||||
// save baro measurement to buffer to be fused later
|
||||
storedBaro.push(baroDataNew);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate filtered offset between baro height measurement and EKF height estimate
|
||||
// offset should be subtracted from baro measurement to match filter estimate
|
||||
// offset is used to enable reversion to baro from alternate height data source
|
||||
void NavEKF3_core::calcFiltBaroOffset()
|
||||
{
|
||||
// Apply a first order LPF with spike protection
|
||||
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
|
||||
}
|
||||
|
||||
// calculate filtered offset between GPS height measurement and EKF height estimate
|
||||
// offset should be subtracted from GPS measurement to match filter estimate
|
||||
// offset is used to switch reversion to GPS from alternate height data source
|
||||
void NavEKF3_core::calcFiltGpsHgtOffset()
|
||||
{
|
||||
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
|
||||
|
||||
// calculate the variance of our a-priori estimate of the ekf origin height
|
||||
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
|
||||
if (activeHgtSource == HGT_SOURCE_BARO) {
|
||||
// Use the baro drift rate
|
||||
const float baroDriftRate = 0.05f;
|
||||
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
|
||||
} else if (activeHgtSource == HGT_SOURCE_RNG) {
|
||||
// use the worse case expected terrain gradient and vehicle horizontal speed
|
||||
const float maxTerrGrad = 0.25f;
|
||||
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
|
||||
} else if (activeHgtSource == HGT_SOURCE_GPS) {
|
||||
// by definition we are using GPS height as the EKF datum in this mode
|
||||
// so cannot run this filter
|
||||
return;
|
||||
}
|
||||
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
|
||||
|
||||
// calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error
|
||||
// when not using GPS as height source
|
||||
float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8];
|
||||
|
||||
// calculate the correction gain
|
||||
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
|
||||
|
||||
// calculate the innovation
|
||||
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
|
||||
|
||||
// check the innovation variance ratio
|
||||
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
|
||||
|
||||
// correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
|
||||
if (ratio < 5.0f) {
|
||||
EKF_origin.alt -= (int)(100.0f * gain * innovation);
|
||||
ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* Air Speed Measurements *
|
||||
********************************************************/
|
||||
|
||||
// check for new airspeed data and update stored measurements if available
|
||||
void NavEKF3_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;
|
||||
|
||||
// Correct for the average intersampling delay due to the filter update rate
|
||||
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// Save data into the buffer to be fused when the fusion time horizon catches up with it
|
||||
storedTAS.push(tasDataNew);
|
||||
}
|
||||
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
|
||||
tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* Range Beacon Measurements *
|
||||
********************************************************/
|
||||
|
||||
// check for new airspeed data and update stored measurements if available
|
||||
void NavEKF3_core::readRngBcnData()
|
||||
{
|
||||
// get the location of the beacon data
|
||||
const AP_Beacon *beacon = _ahrs->get_beacon();
|
||||
|
||||
// exit immediately if no beacon object
|
||||
if (beacon == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get the number of beacons in use
|
||||
N_beacons = beacon->count();
|
||||
|
||||
// search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
|
||||
bool newDataToPush = false;
|
||||
uint8_t numRngBcnsChecked = 0;
|
||||
// start the search one index up from where we left it last time
|
||||
uint8_t index = lastRngBcnChecked;
|
||||
while (!newDataToPush && numRngBcnsChecked < N_beacons) {
|
||||
// track the number of beacons checked
|
||||
numRngBcnsChecked++;
|
||||
|
||||
// move to next beacon, wrap index if necessary
|
||||
index++;
|
||||
if (index >= N_beacons) {
|
||||
index = 0;
|
||||
}
|
||||
|
||||
// check that the beacon is healthy and has new data
|
||||
if (beacon->beacon_healthy(index) &&
|
||||
beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index])
|
||||
{
|
||||
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
|
||||
lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
|
||||
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
|
||||
|
||||
// set the range noise
|
||||
// TODO the range library should provide the noise/accuracy estimate for each beacon
|
||||
rngBcnDataNew.rngErr = frontend->_rngBcnNoise;
|
||||
|
||||
// set the range measurement
|
||||
rngBcnDataNew.rng = beacon->beacon_distance(index);
|
||||
|
||||
// set the beacon position
|
||||
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index);
|
||||
|
||||
// identify the beacon identifier
|
||||
rngBcnDataNew.beacon_ID = index;
|
||||
|
||||
// indicate we have new data to push to the buffer
|
||||
newDataToPush = true;
|
||||
|
||||
// update the last checked index
|
||||
lastRngBcnChecked = index;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if the beacon system has returned a 3D fix
|
||||
if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) {
|
||||
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// Check if the range beacon data can be used to align the vehicle position
|
||||
if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
|
||||
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
|
||||
float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
|
||||
float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
|
||||
if (posDiffSq < 9.0f*posDiffVar) {
|
||||
rngBcnGoodToAlign = true;
|
||||
// Set the EKF origin and magnetic field declination if not previously set
|
||||
if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
|
||||
// get origin from beacon system
|
||||
Location origin_loc;
|
||||
if (beacon->get_origin(origin_loc)) {
|
||||
setOriginLLH(origin_loc);
|
||||
|
||||
// set the NE earth magnetic field states using the published declination
|
||||
// and set the corresponding variances and covariances
|
||||
alignMagStateDeclination();
|
||||
|
||||
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
|
||||
EKF_origin.alt = origin_loc.alt - baroDataNew.hgt;
|
||||
|
||||
// Set the uncertainty of the origin height
|
||||
ekfOriginHgtVar = sq(beaconVehiclePosErr);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rngBcnGoodToAlign = false;
|
||||
}
|
||||
} else {
|
||||
rngBcnGoodToAlign = false;
|
||||
}
|
||||
|
||||
// Save data into the buffer to be fused when the fusion time horizon catches up with it
|
||||
if (newDataToPush) {
|
||||
storedRangeBeacon.push(rngBcnDataNew);
|
||||
}
|
||||
|
||||
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
|
||||
rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
|
||||
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
717
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
Normal file
717
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
Normal file
@ -0,0 +1,717 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/********************************************************
|
||||
* RESET FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
/********************************************************
|
||||
* FUSE MEASURED_DATA *
|
||||
********************************************************/
|
||||
|
||||
// select fusion of optical flow measurements
|
||||
void NavEKF3_core::SelectFlowFusion()
|
||||
{
|
||||
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
||||
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
||||
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
||||
if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
|
||||
optFlowFusionDelayed = true;
|
||||
return;
|
||||
} else {
|
||||
optFlowFusionDelayed = false;
|
||||
}
|
||||
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseOptFlow);
|
||||
// Perform Data Checks
|
||||
// Check if the optical flow data is still valid
|
||||
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
|
||||
// check is the terrain offset estimate is still valid
|
||||
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
|
||||
// Perform tilt check
|
||||
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
|
||||
// Constrain measurements to zero if takeoff is not detected and the height above ground
|
||||
// is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
|
||||
// and carried to test optical flow operation
|
||||
if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
|
||||
ofDataDelayed.flowRadXYcomp.zero();
|
||||
ofDataDelayed.flowRadXY.zero();
|
||||
flowDataValid = true;
|
||||
}
|
||||
|
||||
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
|
||||
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
|
||||
if ((flowDataToFuse || rangeDataToFuse) && tiltOK) {
|
||||
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
|
||||
fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse);
|
||||
// Estimate the terrain offset (runs a one state EKF)
|
||||
EstimateTerrainOffset();
|
||||
}
|
||||
|
||||
// Fuse optical flow data into the main filter
|
||||
if (flowDataToFuse && tiltOK)
|
||||
{
|
||||
// Set the flow noise used by the fusion processes
|
||||
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
|
||||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||||
FuseOptFlow();
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
flowDataToFuse = false;
|
||||
}
|
||||
|
||||
// stop the performance timer
|
||||
hal.util->perf_end(_perf_FuseOptFlow);
|
||||
}
|
||||
|
||||
/*
|
||||
Estimation of terrain offset using a single state EKF
|
||||
The filter can fuse motion compensated optiocal flow rates and range finder measurements
|
||||
*/
|
||||
void NavEKF3_core::EstimateTerrainOffset()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_TerrainOffset);
|
||||
|
||||
// constrain height above ground to be above range measured on ground
|
||||
float heightAboveGndEst = MAX((terrainState - stateStruct.position.z), rngOnGnd);
|
||||
|
||||
// calculate a predicted LOS rate squared
|
||||
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
|
||||
float losRateSq = velHorizSq / sq(heightAboveGndEst);
|
||||
|
||||
// don't update terrain offset state if there is no range finder
|
||||
// don't update terrain state if not generating enough LOS rate, or without GPS, as it is poorly observable
|
||||
// don't update terrain state if we are using it as a height reference in the main filter
|
||||
bool cantFuseFlowData = (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f);
|
||||
if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) {
|
||||
// skip update
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndState = false;
|
||||
// record the time we last updated the terrain offset state
|
||||
gndHgtValidTime_ms = imuSampleTime_ms;
|
||||
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
|
||||
distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
|
||||
prevPosN = stateStruct.position[0];
|
||||
prevPosE = stateStruct.position[1];
|
||||
|
||||
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
|
||||
float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||||
float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[6][6];
|
||||
Popt += Pincrement;
|
||||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||||
|
||||
// fuse range finder data
|
||||
if (rangeDataToFuse) {
|
||||
// predict range
|
||||
float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
|
||||
|
||||
// Copy required states to local variable names
|
||||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
|
||||
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
float R_RNG = frontend->_rngNoise;
|
||||
|
||||
// calculate Kalman gain
|
||||
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
|
||||
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = predRngMeas - rangeDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((sq(innovRng)*SK_RNG) < 25.0f)
|
||||
{
|
||||
// correct the state
|
||||
terrainState -= K_RNG * innovRng;
|
||||
|
||||
// constrain the state
|
||||
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
|
||||
|
||||
// correct the covariance
|
||||
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
|
||||
|
||||
// prevent the state variance from becoming negative
|
||||
Popt = MAX(Popt,0.0f);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (fuseOptFlowData && !cantFuseFlowData) {
|
||||
|
||||
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||||
float losPred; // predicted optical flow angular rate measurement
|
||||
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
|
||||
float K_OPT;
|
||||
float H_OPT;
|
||||
|
||||
// predict range to centre of image
|
||||
float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = prevTnb*stateStruct.velocity;
|
||||
|
||||
// divide velocity by range, subtract body rates and apply scale factor to
|
||||
// get predicted sensed angular optical rates relative to X and Y sensor axes
|
||||
losPred = relVelSensor.length()/flowRngPred;
|
||||
|
||||
// calculate innovations
|
||||
auxFlowObsInnov = losPred - sqrtf(sq(flowRadXYcomp[0]) + sq(flowRadXYcomp[1]));
|
||||
|
||||
// calculate observation jacobian
|
||||
float t3 = sq(q0);
|
||||
float t4 = sq(q1);
|
||||
float t5 = sq(q2);
|
||||
float t6 = sq(q3);
|
||||
float t10 = q0*q3*2.0f;
|
||||
float t11 = q1*q2*2.0f;
|
||||
float t14 = t3+t4-t5-t6;
|
||||
float t15 = t14*stateStruct.velocity.x;
|
||||
float t16 = t10+t11;
|
||||
float t17 = t16*stateStruct.velocity.y;
|
||||
float t18 = q0*q2*2.0f;
|
||||
float t19 = q1*q3*2.0f;
|
||||
float t20 = t18-t19;
|
||||
float t21 = t20*stateStruct.velocity.z;
|
||||
float t2 = t15+t17-t21;
|
||||
float t7 = t3-t4-t5+t6;
|
||||
float t8 = stateStruct.position[2]-terrainState;
|
||||
float t9 = 1.0f/sq(t8);
|
||||
float t24 = t3-t4+t5-t6;
|
||||
float t25 = t24*stateStruct.velocity.y;
|
||||
float t26 = t10-t11;
|
||||
float t27 = t26*stateStruct.velocity.x;
|
||||
float t28 = q0*q1*2.0f;
|
||||
float t29 = q2*q3*2.0f;
|
||||
float t30 = t28+t29;
|
||||
float t31 = t30*stateStruct.velocity.z;
|
||||
float t12 = t25-t27+t31;
|
||||
float t13 = sq(t7);
|
||||
float t22 = sq(t2);
|
||||
float t23 = 1.0f/(t8*t8*t8);
|
||||
float t32 = sq(t12);
|
||||
H_OPT = 0.5f*(t13*t22*t23*2.0f+t13*t23*t32*2.0f)/sqrtf(t9*t13*t22+t9*t13*t32);
|
||||
|
||||
// calculate innovation variances
|
||||
auxFlowObsInnovVar = H_OPT*Popt*H_OPT + R_LOS;
|
||||
|
||||
// calculate Kalman gain
|
||||
K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (MAX(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) {
|
||||
|
||||
// correct the state
|
||||
terrainState -= K_OPT * auxFlowObsInnov;
|
||||
|
||||
// constrain the state
|
||||
terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
|
||||
|
||||
// correct the covariance
|
||||
Popt = Popt - K_OPT * H_OPT * Popt;
|
||||
|
||||
// prevent the state variances from becoming negative
|
||||
Popt = MAX(Popt,0.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// stop the performance timer
|
||||
hal.util->perf_end(_perf_TerrainOffset);
|
||||
}
|
||||
|
||||
/*
|
||||
* Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox.
|
||||
* The script file used to generate these and other equations in this filter can be found here:
|
||||
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
|
||||
* Requires a valid terrain height estimate.
|
||||
*/
|
||||
void NavEKF3_core::FuseOptFlow()
|
||||
{
|
||||
Vector24 H_LOS;
|
||||
Vector3f relVelSensor;
|
||||
Vector14 SH_LOS;
|
||||
Vector2 losPred;
|
||||
|
||||
// Copy required states to local variable names
|
||||
float q0 = stateStruct.quat[0];
|
||||
float q1 = stateStruct.quat[1];
|
||||
float q2 = stateStruct.quat[2];
|
||||
float q3 = stateStruct.quat[3];
|
||||
float vn = stateStruct.velocity.x;
|
||||
float ve = stateStruct.velocity.y;
|
||||
float vd = stateStruct.velocity.z;
|
||||
float pd = stateStruct.position.z;
|
||||
|
||||
// constrain height above ground to be above range measured on ground
|
||||
float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
|
||||
float ptd = pd + heightAboveGndEst;
|
||||
|
||||
// Calculate common expressions for observation jacobians
|
||||
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
|
||||
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
|
||||
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
SH_LOS[7] = q0*q0;
|
||||
SH_LOS[8] = q1*q1;
|
||||
SH_LOS[9] = q2*q2;
|
||||
SH_LOS[10] = q3*q3;
|
||||
SH_LOS[11] = q0*q3*2.0f;
|
||||
SH_LOS[12] = pd-ptd;
|
||||
SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
|
||||
|
||||
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
|
||||
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
|
||||
|
||||
// correct range for flow sensor offset body frame position offset
|
||||
// the corrected value is the predicted range from the sensor focal point to the
|
||||
// centre of the image on the ground assuming flat terrain
|
||||
Vector3f posOffsetBody = (*ofDataDelayed.body_offset) - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
range -= posOffsetEarth.z / prevTnb.c.z;
|
||||
}
|
||||
|
||||
// calculate relative velocity in sensor frame including the relative motion due to rotation
|
||||
relVelSensor = (prevTnb * stateStruct.velocity) + (ofDataDelayed.bodyRadXYZ % posOffsetBody);
|
||||
|
||||
// divide velocity by range to get predicted angular LOS rates relative to X and Y axes
|
||||
losPred[0] = relVelSensor.y/range;
|
||||
losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
// calculate observation jacobians and Kalman gains
|
||||
memset(&H_LOS[0], 0, sizeof(H_LOS));
|
||||
if (obsIndex == 0) {
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
||||
H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P[1][1]*t2*t15;
|
||||
float t32 = P[6][0]*t2*t10;
|
||||
float t33 = P[1][0]*t2*t15;
|
||||
float t34 = P[2][0]*t2*t19;
|
||||
float t35 = P[5][0]*t2*t27;
|
||||
float t79 = P[4][0]*t2*t7;
|
||||
float t80 = P[3][0]*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[6][1]*t2*t10;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[2][1]*t2*t19;
|
||||
float t41 = P[5][1]*t2*t27;
|
||||
float t81 = P[4][1]*t2*t7;
|
||||
float t82 = P[3][1]*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P[6][2]*t2*t10;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t15;
|
||||
float t47 = P[2][2]*t2*t19;
|
||||
float t48 = P[5][2]*t2*t27;
|
||||
float t83 = P[4][2]*t2*t7;
|
||||
float t84 = P[3][2]*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P[6][3]*t2*t10;
|
||||
float t52 = P[0][3]*t2*t5;
|
||||
float t53 = P[1][3]*t2*t15;
|
||||
float t54 = P[2][3]*t2*t19;
|
||||
float t55 = P[5][3]*t2*t27;
|
||||
float t85 = P[4][3]*t2*t7;
|
||||
float t86 = P[3][3]*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P[6][5]*t2*t10;
|
||||
float t58 = P[0][5]*t2*t5;
|
||||
float t59 = P[1][5]*t2*t15;
|
||||
float t60 = P[2][5]*t2*t19;
|
||||
float t61 = P[5][5]*t2*t27;
|
||||
float t88 = P[4][5]*t2*t7;
|
||||
float t89 = P[3][5]*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[6][4]*t2*t10;
|
||||
float t65 = P[0][4]*t2*t5;
|
||||
float t66 = P[1][4]*t2*t15;
|
||||
float t67 = P[2][4]*t2*t19;
|
||||
float t68 = P[5][4]*t2*t27;
|
||||
float t90 = P[4][4]*t2*t7;
|
||||
float t91 = P[3][4]*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P[6][6]*t2*t10;
|
||||
float t71 = P[0][6]*t2*t5;
|
||||
float t72 = P[1][6]*t2*t15;
|
||||
float t73 = P[2][6]*t2*t19;
|
||||
float t74 = P[5][6]*t2*t27;
|
||||
float t93 = P[4][6]*t2*t7;
|
||||
float t94 = P[3][6]*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78;
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
if (t77 > R_LOS) {
|
||||
t78 = 1.0f/t77;
|
||||
faultStatus.bad_xflow = false;
|
||||
} else {
|
||||
t77 = R_LOS;
|
||||
t78 = 1.0f/R_LOS;
|
||||
faultStatus.bad_xflow = true;
|
||||
return;
|
||||
}
|
||||
varInnovOptFlow[0] = t77;
|
||||
|
||||
// calculate innovation for X axis observation
|
||||
innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
|
||||
|
||||
// calculate Kalman gains for X-axis observation
|
||||
Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
Kfusion[1] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
Kfusion[2] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
Kfusion[3] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
Kfusion[4] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
Kfusion[5] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
Kfusion[6] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
Kfusion[7] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
Kfusion[8] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
Kfusion[9] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
Kfusion[10] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
Kfusion[13] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
Kfusion[14] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
Kfusion[15] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
Kfusion[17] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
Kfusion[18] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
Kfusion[19] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
} else {
|
||||
for (uint8_t i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P[1][1]*t2*t16;
|
||||
float t32 = P[5][0]*t2*t8;
|
||||
float t33 = P[1][0]*t2*t16;
|
||||
float t34 = P[3][0]*t2*t22;
|
||||
float t35 = P[4][0]*t2*t27;
|
||||
float t80 = P[6][0]*t2*t10;
|
||||
float t81 = P[2][0]*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[5][1]*t2*t8;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[3][1]*t2*t22;
|
||||
float t41 = P[4][1]*t2*t27;
|
||||
float t82 = P[6][1]*t2*t10;
|
||||
float t83 = P[2][1]*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P[5][2]*t2*t8;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t16;
|
||||
float t47 = P[3][2]*t2*t22;
|
||||
float t48 = P[4][2]*t2*t27;
|
||||
float t79 = P[2][2]*t2*t19;
|
||||
float t84 = P[6][2]*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P[5][3]*t2*t8;
|
||||
float t51 = P[0][3]*t2*t5;
|
||||
float t52 = P[1][3]*t2*t16;
|
||||
float t53 = P[3][3]*t2*t22;
|
||||
float t54 = P[4][3]*t2*t27;
|
||||
float t86 = P[6][3]*t2*t10;
|
||||
float t87 = P[2][3]*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P[5][4]*t2*t8;
|
||||
float t58 = P[0][4]*t2*t5;
|
||||
float t59 = P[1][4]*t2*t16;
|
||||
float t60 = P[3][4]*t2*t22;
|
||||
float t61 = P[4][4]*t2*t27;
|
||||
float t88 = P[6][4]*t2*t10;
|
||||
float t89 = P[2][4]*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[5][5]*t2*t8;
|
||||
float t65 = P[0][5]*t2*t5;
|
||||
float t66 = P[1][5]*t2*t16;
|
||||
float t67 = P[3][5]*t2*t22;
|
||||
float t68 = P[4][5]*t2*t27;
|
||||
float t90 = P[6][5]*t2*t10;
|
||||
float t91 = P[2][5]*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P[5][6]*t2*t8;
|
||||
float t72 = P[0][6]*t2*t5;
|
||||
float t73 = P[1][6]*t2*t16;
|
||||
float t74 = P[3][6]*t2*t22;
|
||||
float t75 = P[4][6]*t2*t27;
|
||||
float t92 = P[6][6]*t2*t10;
|
||||
float t93 = P[2][6]*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
float t78;
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
if (t77 > R_LOS) {
|
||||
t78 = 1.0f/t77;
|
||||
faultStatus.bad_yflow = false;
|
||||
} else {
|
||||
t77 = R_LOS;
|
||||
t78 = 1.0f/R_LOS;
|
||||
faultStatus.bad_yflow = true;
|
||||
return;
|
||||
}
|
||||
varInnovOptFlow[1] = t77;
|
||||
|
||||
// calculate innovation for Y observation
|
||||
innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
|
||||
|
||||
// calculate Kalman gains for the Y-axis observation
|
||||
Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
Kfusion[1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
Kfusion[2] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
Kfusion[3] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
Kfusion[4] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
Kfusion[5] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
Kfusion[6] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
Kfusion[7] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
Kfusion[8] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
Kfusion[9] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
Kfusion[10] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
Kfusion[13] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
Kfusion[14] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
Kfusion[15] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
Kfusion[17] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
Kfusion[18] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
Kfusion[19] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
} else {
|
||||
for (uint8_t i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
|
||||
if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
|
||||
// record the last time observations were accepted for fusion
|
||||
prevFlowFuseTime_ms = imuSampleTime_ms;
|
||||
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||
for (unsigned j = 0; j<=6; j++) {
|
||||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
}
|
||||
for (unsigned j = 7; j<=stateIndexLim; j++) {
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
}
|
||||
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||
ftype res = 0;
|
||||
res += KH[i][0] * P[0][j];
|
||||
res += KH[i][1] * P[1][j];
|
||||
res += KH[i][2] * P[2][j];
|
||||
res += KH[i][3] * P[3][j];
|
||||
res += KH[i][4] * P[4][j];
|
||||
res += KH[i][5] * P[5][j];
|
||||
res += KH[i][6] * P[6][j];
|
||||
KHP[i][j] = res;
|
||||
}
|
||||
}
|
||||
|
||||
// Check that we are not going to drive any variances negative and skip the update if so
|
||||
bool healthyFusion = true;
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
if (KHP[i][i] > P[i][i]) {
|
||||
healthyFusion = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (healthyFusion) {
|
||||
// update the covariance matrix
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
}
|
||||
stateStruct.quat.normalize();
|
||||
|
||||
} else {
|
||||
// record bad axis
|
||||
if (obsIndex == 0) {
|
||||
faultStatus.bad_xflow = true;
|
||||
} else if (obsIndex == 1) {
|
||||
faultStatus.bad_yflow = true;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* MISC FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
588
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Normal file
588
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Normal file
@ -0,0 +1,588 @@
|
||||
/// -*- 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
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool NavEKF3_core::healthy(void) const
|
||||
{
|
||||
uint16_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;
|
||||
}
|
||||
// position and height innovations must be within limits when on-ground and in a static mode of operation
|
||||
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
|
||||
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// all OK
|
||||
return true;
|
||||
}
|
||||
|
||||
// Return a consolidated error score where higher numbers represent larger errors
|
||||
// Intended to be used by the front-end to determine which is the primary EKF
|
||||
float NavEKF3_core::errorScore() const
|
||||
{
|
||||
float score = 0.0f;
|
||||
if (tiltAlignComplete && yawAlignComplete) {
|
||||
// Check GPS fusion performance
|
||||
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
|
||||
// Check altimeter fusion performance
|
||||
score = MAX(score, hgtTestRatio);
|
||||
}
|
||||
return score;
|
||||
}
|
||||
|
||||
// return data for debugging optical flow fusion
|
||||
void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
|
||||
{
|
||||
varFlow = MAX(flowTestRatio[0],flowTestRatio[1]);
|
||||
gndOffset = terrainState;
|
||||
flowInnovX = innovOptFlow[0];
|
||||
flowInnovY = innovOptFlow[1];
|
||||
auxInnov = auxFlowObsInnov;
|
||||
HAGL = terrainState - stateStruct.position.z;
|
||||
rngInnov = innovRng;
|
||||
range = rangeDataDelayed.rng;
|
||||
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
}
|
||||
|
||||
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
|
||||
bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
|
||||
{
|
||||
// if the states have not been initialised or we have not received any beacon updates then return zeros
|
||||
if (!statesInitialised || N_beacons == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
|
||||
if (rngBcnFuseDataReportIndex >= N_beacons) {
|
||||
rngBcnFuseDataReportIndex = 0;
|
||||
}
|
||||
|
||||
// Output the fusion status data for the specified beacon
|
||||
ID = rngBcnFuseDataReportIndex; // beacon identifier
|
||||
rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng; // measured range to beacon (m)
|
||||
innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m)
|
||||
innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2)
|
||||
testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio
|
||||
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon NED position
|
||||
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate
|
||||
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate
|
||||
rngBcnFuseDataReportIndex++;
|
||||
return true;
|
||||
}
|
||||
|
||||
// provides the height limit to be observed by the control loops
|
||||
// returns false if no height limiting is required
|
||||
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation
|
||||
bool NavEKF3_core::getHeightControlLimit(float &height) const
|
||||
{
|
||||
// only ask for limiting if we are doing optical flow navigation
|
||||
if (frontend->_fusionModeGPS == 3) {
|
||||
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
|
||||
height = MAX(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f);
|
||||
// If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
|
||||
if (frontend->_altSource != 1) {
|
||||
height -= terrainState;
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// return the Euler roll, pitch and yaw angle in radians
|
||||
void NavEKF3_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 NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
|
||||
{
|
||||
if (dtEkfAvg < 1e-6f) {
|
||||
gyroBias.zero();
|
||||
return;
|
||||
}
|
||||
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
|
||||
}
|
||||
|
||||
// return accelerometer bias in m/s/s
|
||||
void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
|
||||
{
|
||||
if (!statesInitialised) {
|
||||
accelBias.zero();
|
||||
return;
|
||||
}
|
||||
accelBias = stateStruct.accel_bias / dtEkfAvg;
|
||||
}
|
||||
|
||||
// return tilt error convergence metric
|
||||
void NavEKF3_core::getTiltError(float &ang) const
|
||||
{
|
||||
ang = stateStruct.quat.length();
|
||||
}
|
||||
|
||||
// return the transformation matrix from XYZ (body) to NED axes
|
||||
void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
|
||||
{
|
||||
outputDataNew.quat.rotation_matrix(mat);
|
||||
mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body();
|
||||
}
|
||||
|
||||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
||||
void NavEKF3_core::getQuaternion(Quaternion& ret) const
|
||||
{
|
||||
ret = outputDataNew.quat;
|
||||
}
|
||||
|
||||
// return the amount of yaw angle change due to the last yaw angle reset in radians
|
||||
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const
|
||||
{
|
||||
yawAng = yawResetAngle;
|
||||
return lastYawReset_ms;
|
||||
}
|
||||
|
||||
// return the amount of NE position change due to the last position reset in metres
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const
|
||||
{
|
||||
pos = posResetNE;
|
||||
return lastPosReset_ms;
|
||||
}
|
||||
|
||||
// return the amount of vertical position change due to the last vertical position reset in metres
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const
|
||||
{
|
||||
posD = posResetD;
|
||||
return lastPosResetD_ms;
|
||||
}
|
||||
|
||||
// return the amount of NE velocity change due to the last velocity reset in metres/sec
|
||||
// returns the time of the last reset or 0 if no reset has ever occurred
|
||||
uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const
|
||||
{
|
||||
vel = velResetNE;
|
||||
return lastVelReset_ms;
|
||||
}
|
||||
|
||||
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
|
||||
void NavEKF3_core::getWind(Vector3f &wind) const
|
||||
{
|
||||
wind.x = stateStruct.wind_vel.x;
|
||||
wind.y = stateStruct.wind_vel.y;
|
||||
wind.z = 0.0f; // currently don't estimate this
|
||||
}
|
||||
|
||||
|
||||
// return the NED velocity of the body frame origin in m/s
|
||||
//
|
||||
void NavEKF3_core::getVelNED(Vector3f &vel) const
|
||||
{
|
||||
// correct for the IMU position offset (EKF calculations are at the IMU)
|
||||
vel = outputDataNew.velocity + velOffsetNED;
|
||||
}
|
||||
|
||||
// Return the rate of change of vertical position in the down diection (dPosD/dt) of the body frame origin in m/s
|
||||
float NavEKF3_core::getPosDownDerivative(void) const
|
||||
{
|
||||
// return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
|
||||
// correct for the IMU offset (EKF calculations are at the IMU)
|
||||
return posDownDerivative + velOffsetNED.z;
|
||||
}
|
||||
|
||||
// This returns the specific forces in the NED frame
|
||||
void NavEKF3_core::getAccelNED(Vector3f &accelNED) const {
|
||||
accelNED = velDotNED;
|
||||
accelNED.z -= GRAVITY_MSS;
|
||||
}
|
||||
|
||||
// Write the last estimated NE position of the body frame origin relative to the reference point (m).
|
||||
// Return true if the estimate is valid
|
||||
bool NavEKF3_core::getPosNE(Vector2f &posNE) const
|
||||
{
|
||||
// There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
|
||||
if (PV_AidingMode != AID_NONE) {
|
||||
// This is the normal mode of operation where we can use the EKF position states
|
||||
// correct for the IMU offset (EKF calculations are at the IMU)
|
||||
posNE.x = outputDataNew.position.x + posOffsetNED.x;
|
||||
posNE.y = outputDataNew.position.y + posOffsetNED.y;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
|
||||
if(validOrigin) {
|
||||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
|
||||
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
|
||||
posNE.x = tempPosNE.x;
|
||||
posNE.y = tempPosNE.y;
|
||||
return false;
|
||||
} else if (rngBcnAlignmentStarted) {
|
||||
// If we are attempting alignment using range beacon data, then report the position
|
||||
posNE.x = receiverPos.x;
|
||||
posNE.y = receiverPos.y;
|
||||
return false;
|
||||
} else {
|
||||
// If no GPS fix is available, all we can do is provide the last known position
|
||||
posNE.x = outputDataNew.position.x;
|
||||
posNE.y = outputDataNew.position.y;
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// If the origin has not been set, then we have no means of providing a relative position
|
||||
posNE.x = 0.0f;
|
||||
posNE.y = 0.0f;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Write the last calculated D position of the body frame origin relative to the reference point (m).
|
||||
// Return true if the estimate is valid
|
||||
bool NavEKF3_core::getPosD(float &posD) const
|
||||
{
|
||||
// The EKF always has a height estimate regardless of mode of operation
|
||||
// correct for the IMU offset (EKF calculations are at the IMU)
|
||||
posD = outputDataNew.position.z + posOffsetNED.z;
|
||||
|
||||
// Return the current height solution status
|
||||
return filterStatus.flags.vert_pos;
|
||||
|
||||
}
|
||||
|
||||
// return the estimated height of body frame origin above ground level
|
||||
bool NavEKF3_core::getHAGL(float &HAGL) const
|
||||
{
|
||||
HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
|
||||
// If we know the terrain offset and altitude, then we have a valid height above ground estimate
|
||||
return !hgtTimeout && gndOffsetValid && healthy();
|
||||
}
|
||||
|
||||
// Return the last calculated latitude, longitude and height in WGS-84
|
||||
// If a calculated location isn't available, return a raw GPS measurement
|
||||
// The status will return true if a calculation or raw measurement is available
|
||||
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
|
||||
bool NavEKF3_core::getLLH(struct Location &loc) const
|
||||
{
|
||||
if(validOrigin) {
|
||||
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
|
||||
loc.alt = EKF_origin.alt - outputDataNew.position.z*100;
|
||||
loc.flags.relative_alt = 0;
|
||||
loc.flags.terrain_alt = 0;
|
||||
|
||||
// there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding)
|
||||
if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
|
||||
loc.lat = EKF_origin.lat;
|
||||
loc.lng = EKF_origin.lng;
|
||||
location_offset(loc, outputDataNew.position.x, outputDataNew.position.y);
|
||||
return true;
|
||||
} else {
|
||||
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
|
||||
// in this mode we cannot use the EKF states to estimate position so will return the best available data
|
||||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
|
||||
// we have a GPS position fix to return
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
loc.lat = gpsloc.lat;
|
||||
loc.lng = gpsloc.lng;
|
||||
return true;
|
||||
} else {
|
||||
// if no GPS fix, provide last known position before entering the mode
|
||||
location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw
|
||||
// GPS reading if available and return false
|
||||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) {
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
loc = gpsloc;
|
||||
loc.flags.relative_alt = 0;
|
||||
loc.flags.terrain_alt = 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// return the horizontal speed limit in m/s set by optical flow sensor limits
|
||||
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
|
||||
void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
|
||||
{
|
||||
if (PV_AidingMode == AID_RELATIVE) {
|
||||
// allow 1.0 rad/sec margin for angular motion
|
||||
ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
|
||||
// use standard gains up to 5.0 metres height and reduce above that
|
||||
ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f);
|
||||
} else {
|
||||
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
|
||||
ekfNavVelGainScaler = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// return the LLH location of the filters NED origin
|
||||
bool NavEKF3_core::getOriginLLH(struct Location &loc) const
|
||||
{
|
||||
if (validOrigin) {
|
||||
loc = EKF_origin;
|
||||
}
|
||||
return validOrigin;
|
||||
}
|
||||
|
||||
// return earth magnetic field estimates in measurement units / 1000
|
||||
void NavEKF3_core::getMagNED(Vector3f &magNED) const
|
||||
{
|
||||
magNED = stateStruct.earth_magfield * 1000.0f;
|
||||
}
|
||||
|
||||
// return body magnetic field estimates in measurement units / 1000
|
||||
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
|
||||
{
|
||||
magXYZ = stateStruct.body_magfield*1000.0f;
|
||||
}
|
||||
|
||||
// return magnetometer offsets
|
||||
// return true if offsets are valid
|
||||
bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
||||
{
|
||||
// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
|
||||
// primary compass is valid and state variances have converged
|
||||
const float maxMagVar = 5E-6f;
|
||||
bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
|
||||
if ((mag_idx == magSelectIndex) &&
|
||||
finalInflightMagInit &&
|
||||
!inhibitMagStates &&
|
||||
_ahrs->get_compass()->healthy(magSelectIndex) &&
|
||||
variancesConverged) {
|
||||
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
|
||||
return true;
|
||||
} else {
|
||||
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// return the index for the active magnetometer
|
||||
uint8_t NavEKF3_core::getActiveMag() const
|
||||
{
|
||||
return (uint8_t)magSelectIndex;
|
||||
}
|
||||
|
||||
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
|
||||
void NavEKF3_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 delta in position due to the last position reset
|
||||
void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
|
||||
{
|
||||
velVar = sqrtf(velTestRatio);
|
||||
posVar = sqrtf(posTestRatio);
|
||||
hgtVar = sqrtf(hgtTestRatio);
|
||||
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
|
||||
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
|
||||
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
|
||||
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
|
||||
tasVar = sqrtf(tasTestRatio);
|
||||
offset = posResetNE;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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 NavEKF3_core::getFilterFaults(uint16_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 NavEKF3_core::getFilterTimeouts(uint8_t &timeouts) const
|
||||
{
|
||||
timeouts = (posTimeout<<0 |
|
||||
velTimeout<<1 |
|
||||
hgtTimeout<<2 |
|
||||
magTimeout<<3 |
|
||||
tasTimeout<<4);
|
||||
}
|
||||
|
||||
// Return the navigation filter status message
|
||||
void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
|
||||
{
|
||||
status = filterStatus;
|
||||
}
|
||||
|
||||
/*
|
||||
return filter gps quality check status
|
||||
*/
|
||||
void NavEKF3_core::getFilterGpsStatus(nav_gps_status &faults) const
|
||||
{
|
||||
// init return value
|
||||
faults.value = 0;
|
||||
|
||||
// set individual flags
|
||||
faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient
|
||||
faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient
|
||||
faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use
|
||||
faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient
|
||||
faults.flags.bad_horiz_drift = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static)
|
||||
faults.flags.bad_hdop = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS
|
||||
faults.flags.bad_vert_vel = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static)
|
||||
faults.flags.bad_fix = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required
|
||||
faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static)
|
||||
}
|
||||
|
||||
// send an EKF_STATUS message to GCS
|
||||
void NavEKF3_core::send_status_report(mavlink_channel_t chan)
|
||||
{
|
||||
// prepare flags
|
||||
uint16_t flags = 0;
|
||||
if (filterStatus.flags.attitude) {
|
||||
flags |= EKF_ATTITUDE;
|
||||
}
|
||||
if (filterStatus.flags.horiz_vel) {
|
||||
flags |= EKF_VELOCITY_HORIZ;
|
||||
}
|
||||
if (filterStatus.flags.vert_vel) {
|
||||
flags |= EKF_VELOCITY_VERT;
|
||||
}
|
||||
if (filterStatus.flags.horiz_pos_rel) {
|
||||
flags |= EKF_POS_HORIZ_REL;
|
||||
}
|
||||
if (filterStatus.flags.horiz_pos_abs) {
|
||||
flags |= EKF_POS_HORIZ_ABS;
|
||||
}
|
||||
if (filterStatus.flags.vert_pos) {
|
||||
flags |= EKF_POS_VERT_ABS;
|
||||
}
|
||||
if (filterStatus.flags.terrain_alt) {
|
||||
flags |= EKF_POS_VERT_AGL;
|
||||
}
|
||||
if (filterStatus.flags.const_pos_mode) {
|
||||
flags |= EKF_CONST_POS_MODE;
|
||||
}
|
||||
if (filterStatus.flags.pred_horiz_pos_rel) {
|
||||
flags |= EKF_PRED_POS_HORIZ_REL;
|
||||
}
|
||||
if (filterStatus.flags.pred_horiz_pos_abs) {
|
||||
flags |= EKF_PRED_POS_HORIZ_ABS;
|
||||
}
|
||||
if (filterStatus.flags.gps_glitching) {
|
||||
flags |= (1<<15);
|
||||
}
|
||||
|
||||
// get variances
|
||||
float velVar, posVar, hgtVar, tasVar;
|
||||
Vector3f magVar;
|
||||
Vector2f offset;
|
||||
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
|
||||
// Only report range finder normalised innovation levels if the EKF needs the data for primary
|
||||
// height estimation or optical flow operation. This prevents false alarms at the GCS if a
|
||||
// range finder is fitted for other applications
|
||||
float temp;
|
||||
if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE || flowDataValid) {
|
||||
temp = sqrtf(auxRngTestRatio);
|
||||
} else {
|
||||
temp = 0.0f;
|
||||
}
|
||||
|
||||
// send message
|
||||
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp);
|
||||
|
||||
}
|
||||
|
||||
// report the reason for why the backend is refusing to initialise
|
||||
const char *NavEKF3_core::prearm_failure_reason(void) const
|
||||
{
|
||||
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
|
||||
// we are not failing
|
||||
return nullptr;
|
||||
}
|
||||
return prearm_fail_string;
|
||||
}
|
||||
|
||||
|
||||
// report the number of frames lapsed since the last state prediction
|
||||
// this is used by other instances to level load
|
||||
uint8_t NavEKF3_core::getFramesSincePredict(void) const
|
||||
{
|
||||
return framesSincePredict;
|
||||
}
|
||||
|
||||
// publish output observer angular, velocity and position tracking error
|
||||
void NavEKF3_core::getOutputTrackingError(Vector3f &error) const
|
||||
{
|
||||
error = outputTrackError;
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
819
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Normal file
819
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Normal file
@ -0,0 +1,819 @@
|
||||
/// -*- 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
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/********************************************************
|
||||
* RESET FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
|
||||
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
|
||||
void NavEKF3_core::ResetVelocity(void)
|
||||
{
|
||||
// Store the position before the reset so that we can record the reset delta
|
||||
velResetNE.x = stateStruct.velocity.x;
|
||||
velResetNE.y = stateStruct.velocity.y;
|
||||
|
||||
// reset the corresponding covariances
|
||||
zeroRows(P,4,5);
|
||||
zeroCols(P,4,5);
|
||||
|
||||
if (PV_AidingMode != AID_ABSOLUTE) {
|
||||
stateStruct.velocity.zero();
|
||||
// set the variances using the measurement noise parameter
|
||||
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
|
||||
} else {
|
||||
// reset horizontal velocity states to the GPS velocity if available
|
||||
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
||||
stateStruct.velocity.x = gpsDataNew.vel.x;
|
||||
stateStruct.velocity.y = gpsDataNew.vel.y;
|
||||
// set the variances using the reported GPS speed accuracy
|
||||
P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
|
||||
// clear the timeout flags and counters
|
||||
velTimeout = false;
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
} else {
|
||||
stateStruct.velocity.x = 0.0f;
|
||||
stateStruct.velocity.y = 0.0f;
|
||||
// set the variances using the likely speed range
|
||||
P[5][5] = P[4][4] = sq(25.0f);
|
||||
// clear the timeout flags and counters
|
||||
velTimeout = false;
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
storedOutput[i].velocity.x = stateStruct.velocity.x;
|
||||
storedOutput[i].velocity.y = stateStruct.velocity.y;
|
||||
}
|
||||
outputDataNew.velocity.x = stateStruct.velocity.x;
|
||||
outputDataNew.velocity.y = stateStruct.velocity.y;
|
||||
outputDataDelayed.velocity.x = stateStruct.velocity.x;
|
||||
outputDataDelayed.velocity.y = stateStruct.velocity.y;
|
||||
|
||||
// Calculate the position jump due to the reset
|
||||
velResetNE.x = stateStruct.velocity.x - velResetNE.x;
|
||||
velResetNE.y = stateStruct.velocity.y - velResetNE.y;
|
||||
|
||||
// store the time of the reset
|
||||
lastVelReset_ms = imuSampleTime_ms;
|
||||
|
||||
|
||||
}
|
||||
|
||||
// resets position states to last GPS measurement or to zero if in constant position mode
|
||||
void NavEKF3_core::ResetPosition(void)
|
||||
{
|
||||
// Store the position before the reset so that we can record the reset delta
|
||||
posResetNE.x = stateStruct.position.x;
|
||||
posResetNE.y = stateStruct.position.y;
|
||||
|
||||
// reset the corresponding covariances
|
||||
zeroRows(P,7,8);
|
||||
zeroCols(P,7,8);
|
||||
|
||||
if (PV_AidingMode != AID_ABSOLUTE) {
|
||||
// reset all position state history to the last known position
|
||||
stateStruct.position.x = lastKnownPositionNE.x;
|
||||
stateStruct.position.y = lastKnownPositionNE.y;
|
||||
// set the variances using the position measurement noise parameter
|
||||
P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
|
||||
} else {
|
||||
// Use GPS data as first preference if fresh data is available
|
||||
if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
|
||||
// write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
|
||||
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
||||
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
|
||||
// set the variances using the position measurement noise parameter
|
||||
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
|
||||
// clear the timeout flags and counters
|
||||
posTimeout = false;
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
} else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
|
||||
// use the range beacon data as a second preference
|
||||
stateStruct.position.x = receiverPos.x;
|
||||
stateStruct.position.y = receiverPos.y;
|
||||
// set the variances from the beacon alignment filter
|
||||
P[7][7] = receiverPosCov[0][0];
|
||||
P[8][8] = receiverPosCov[1][1];
|
||||
// clear the timeout flags and counters
|
||||
rngBcnTimeout = false;
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
storedOutput[i].position.x = stateStruct.position.x;
|
||||
storedOutput[i].position.y = stateStruct.position.y;
|
||||
}
|
||||
outputDataNew.position.x = stateStruct.position.x;
|
||||
outputDataNew.position.y = stateStruct.position.y;
|
||||
outputDataDelayed.position.x = stateStruct.position.x;
|
||||
outputDataDelayed.position.y = stateStruct.position.y;
|
||||
|
||||
// Calculate the position jump due to the reset
|
||||
posResetNE.x = stateStruct.position.x - posResetNE.x;
|
||||
posResetNE.y = stateStruct.position.y - posResetNE.y;
|
||||
|
||||
// store the time of the reset
|
||||
lastPosReset_ms = imuSampleTime_ms;
|
||||
|
||||
}
|
||||
|
||||
// reset the vertical position state using the last height measurement
|
||||
void NavEKF3_core::ResetHeight(void)
|
||||
{
|
||||
// Store the position before the reset so that we can record the reset delta
|
||||
posResetD = stateStruct.position.z;
|
||||
|
||||
// write to the state vector
|
||||
stateStruct.position.z = -hgtMea;
|
||||
outputDataNew.position.z = stateStruct.position.z;
|
||||
outputDataDelayed.position.z = stateStruct.position.z;
|
||||
|
||||
// reset the terrain state height
|
||||
if (onGround) {
|
||||
// assume vehicle is sitting on the ground
|
||||
terrainState = stateStruct.position.z + rngOnGnd;
|
||||
} else {
|
||||
// can make no assumption other than vehicle is not below ground level
|
||||
terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState);
|
||||
}
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
storedOutput[i].position.z = stateStruct.position.z;
|
||||
}
|
||||
|
||||
// Calculate the position jump due to the reset
|
||||
posResetD = stateStruct.position.z - posResetD;
|
||||
|
||||
// store the time of the reset
|
||||
lastPosResetD_ms = imuSampleTime_ms;
|
||||
|
||||
// clear the timeout flags and counters
|
||||
hgtTimeout = false;
|
||||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||||
|
||||
// reset the corresponding covariances
|
||||
zeroRows(P,9,9);
|
||||
zeroCols(P,9,9);
|
||||
|
||||
// set the variances to the measurement variance
|
||||
P[9][9] = posDownObsNoise;
|
||||
|
||||
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
|
||||
// Check that GPS vertical velocity data is available and can be used
|
||||
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0) {
|
||||
stateStruct.velocity.z = gpsDataNew.vel.z;
|
||||
} else if (onGround) {
|
||||
stateStruct.velocity.z = 0.0f;
|
||||
}
|
||||
for (uint8_t i=0; i<imu_buffer_length; i++) {
|
||||
storedOutput[i].velocity.z = stateStruct.velocity.z;
|
||||
}
|
||||
outputDataNew.velocity.z = stateStruct.velocity.z;
|
||||
outputDataDelayed.velocity.z = stateStruct.velocity.z;
|
||||
|
||||
// reset the corresponding covariances
|
||||
zeroRows(P,6,6);
|
||||
zeroCols(P,6,6);
|
||||
|
||||
// set the variances to the measurement variance
|
||||
P[6][6] = sq(frontend->_gpsVertVelNoise);
|
||||
|
||||
}
|
||||
|
||||
// Zero the EKF height datum
|
||||
// Return true if the height datum reset has been performed
|
||||
bool NavEKF3_core::resetHeightDatum(void)
|
||||
{
|
||||
if (activeHgtSource == HGT_SOURCE_RNG) {
|
||||
// by definition the height dataum is at ground level so cannot perform the reset
|
||||
return false;
|
||||
}
|
||||
// record the old height estimate
|
||||
float oldHgt = -stateStruct.position.z;
|
||||
// reset the barometer so that it reads zero at the current height
|
||||
frontend->_baro.update_calibration();
|
||||
// reset the height state
|
||||
stateStruct.position.z = 0.0f;
|
||||
// adjust the height of the EKF origin so that the origin plus baro height before and afer the reset is the same
|
||||
if (validOrigin) {
|
||||
EKF_origin.alt += oldHgt*100;
|
||||
}
|
||||
// adjust the terrain state
|
||||
terrainState += oldHgt;
|
||||
return true;
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* FUSE MEASURED_DATA *
|
||||
********************************************************/
|
||||
// select fusion of velocity, position and height measurements
|
||||
void NavEKF3_core::SelectVelPosFusion()
|
||||
{
|
||||
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
|
||||
// If so, don't fuse measurements on this time step to reduce frame over-runs
|
||||
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
|
||||
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
|
||||
posVelFusionDelayed = true;
|
||||
return;
|
||||
} else {
|
||||
posVelFusionDelayed = false;
|
||||
}
|
||||
|
||||
// read GPS data from the sensor and check for new data in the buffer
|
||||
readGpsData();
|
||||
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
||||
// Determine if we need to fuse position and velocity data on this time step
|
||||
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
||||
// Don't fuse velocity data if GPS doesn't support it
|
||||
if (frontend->_fusionModeGPS <= 1) {
|
||||
fuseVelData = true;
|
||||
} else {
|
||||
fuseVelData = false;
|
||||
}
|
||||
fusePosData = true;
|
||||
|
||||
// correct GPS data for position offset of antenna phase centre relative to the IMU
|
||||
Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
if (fuseVelData) {
|
||||
// TODO use a filtered angular rate with a group delay that matches the GPS delay
|
||||
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
|
||||
Vector3f velOffsetBody = angRate % posOffsetBody;
|
||||
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
|
||||
gpsDataDelayed.vel -= velOffsetEarth;
|
||||
}
|
||||
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
gpsDataDelayed.pos.x -= posOffsetEarth.x;
|
||||
gpsDataDelayed.pos.y -= posOffsetEarth.y;
|
||||
gpsDataDelayed.hgt += posOffsetEarth.z;
|
||||
}
|
||||
} else {
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
}
|
||||
|
||||
// we have GPS data to fuse and a request to align the yaw using the GPS course
|
||||
if (gpsYawResetRequest) {
|
||||
realignYawGPS();
|
||||
}
|
||||
|
||||
// Select height data to be fused from the available baro, range finder and GPS sources
|
||||
selectHeightForFusion();
|
||||
|
||||
// If we are operating without any aiding, fuse in the last known position
|
||||
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
|
||||
// Do this to coincide with the height fusion
|
||||
if (fuseHgtData && PV_AidingMode == AID_NONE) {
|
||||
gpsDataDelayed.vel.zero();
|
||||
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
|
||||
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
|
||||
|
||||
fusePosData = true;
|
||||
fuseVelData = false;
|
||||
}
|
||||
|
||||
// perform fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||||
FuseVelPosNED();
|
||||
// clear the flags to prevent repeated fusion of the same data
|
||||
fuseVelData = false;
|
||||
fuseHgtData = false;
|
||||
fusePosData = false;
|
||||
}
|
||||
}
|
||||
|
||||
// fuse selected position, velocity and height measurements
|
||||
void NavEKF3_core::FuseVelPosNED()
|
||||
{
|
||||
// start performance timer
|
||||
hal.util->perf_begin(_perf_FuseVelPosNED);
|
||||
|
||||
// health is set bad until test passed
|
||||
velHealth = false;
|
||||
posHealth = false;
|
||||
hgtHealth = false;
|
||||
|
||||
// declare variables used to check measurement errors
|
||||
Vector3f velInnov;
|
||||
|
||||
// declare variables used to control access to arrays
|
||||
bool fuseData[6] = {false,false,false,false,false,false};
|
||||
uint8_t stateIndex;
|
||||
uint8_t obsIndex;
|
||||
|
||||
// declare variables used by state and covariance update calculations
|
||||
Vector6 R_OBS; // Measurement variances used for fusion
|
||||
Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
|
||||
Vector6 observation;
|
||||
float SK;
|
||||
|
||||
// perform sequential fusion of GPS measurements. This assumes that the
|
||||
// errors in the different velocity and position components are
|
||||
// uncorrelated which is not true, however in the absence of covariance
|
||||
// data from the GPS receiver it is the only assumption we can make
|
||||
// so we might as well take advantage of the computational efficiencies
|
||||
// associated with sequential fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||||
// form the observation vector
|
||||
observation[0] = gpsDataDelayed.vel.x;
|
||||
observation[1] = gpsDataDelayed.vel.y;
|
||||
observation[2] = gpsDataDelayed.vel.z;
|
||||
observation[3] = gpsDataDelayed.pos.x;
|
||||
observation[4] = gpsDataDelayed.pos.y;
|
||||
observation[5] = -hgtMea;
|
||||
|
||||
// calculate additional error in GPS position caused by manoeuvring
|
||||
float posErr = frontend->gpsPosVarAccScale * accNavMag;
|
||||
|
||||
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
// Use different errors if operating without external aiding using an assumed position or velocity of zero
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
if (tiltAlignComplete && motorsArmed) {
|
||||
// This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
|
||||
R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
|
||||
} else {
|
||||
// Use a smaller value to give faster initial alignment
|
||||
R_OBS[0] = sq(0.5f);
|
||||
}
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[2] = R_OBS[0];
|
||||
R_OBS[3] = R_OBS[0];
|
||||
R_OBS[4] = R_OBS[0];
|
||||
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
||||
} else {
|
||||
if (gpsSpdAccuracy > 0.0f) {
|
||||
// use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
|
||||
R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
|
||||
R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
|
||||
} else {
|
||||
// calculate additional error in GPS velocity caused by manoeuvring
|
||||
R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||
R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
|
||||
}
|
||||
R_OBS[1] = R_OBS[0];
|
||||
// Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
|
||||
if (gpsPosAccuracy > 0.0f) {
|
||||
R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
|
||||
} else {
|
||||
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
|
||||
}
|
||||
R_OBS[4] = R_OBS[3];
|
||||
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
|
||||
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
|
||||
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
|
||||
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
|
||||
}
|
||||
R_OBS[5] = posDownObsNoise;
|
||||
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
|
||||
|
||||
// if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
|
||||
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
|
||||
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
|
||||
if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
|
||||
// calculate innovations for height and vertical GPS vel measurements
|
||||
float hgtErr = stateStruct.position.z - observation[5];
|
||||
float velDErr = stateStruct.velocity.z - observation[2];
|
||||
// check if they are the same sign and both more than 3-sigma out of bounds
|
||||
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
|
||||
badIMUdata = true;
|
||||
} else {
|
||||
badIMUdata = false;
|
||||
}
|
||||
}
|
||||
|
||||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||||
// test position measurements
|
||||
if (fusePosData) {
|
||||
// test horizontal position measurements
|
||||
innovVelPos[3] = stateStruct.position.x - observation[3];
|
||||
innovVelPos[4] = stateStruct.position.y - observation[4];
|
||||
varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3];
|
||||
varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4];
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
|
||||
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
|
||||
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
|
||||
// use position data if healthy or timed out
|
||||
if (PV_AidingMode == AID_NONE) {
|
||||
posHealth = true;
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
} else if (posHealth || posTimeout) {
|
||||
posHealth = true;
|
||||
lastPosPassTime_ms = imuSampleTime_ms;
|
||||
// if timed out or outside the specified uncertainty radius, reset to the GPS
|
||||
if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
|
||||
// reset the position to the current GPS position
|
||||
ResetPosition();
|
||||
// reset the velocity to the GPS velocity
|
||||
ResetVelocity();
|
||||
// don't fuse GPS data on this time step
|
||||
fusePosData = false;
|
||||
fuseVelData = false;
|
||||
// Reset the position variances and corresponding covariances to a value that will pass the checks
|
||||
zeroRows(P,7,8);
|
||||
zeroCols(P,7,8);
|
||||
P[7][7] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
|
||||
P[8][8] = P[7][7];
|
||||
// Reset the normalised innovation to avoid failing the bad fusion tests
|
||||
posTestRatio = 0.0f;
|
||||
velTestRatio = 0.0f;
|
||||
}
|
||||
} else {
|
||||
posHealth = false;
|
||||
}
|
||||
}
|
||||
|
||||
// test velocity measurements
|
||||
if (fuseVelData) {
|
||||
// test velocity measurements
|
||||
uint8_t imax = 2;
|
||||
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
|
||||
if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
|
||||
imax = 1;
|
||||
}
|
||||
float innovVelSumSq = 0; // sum of squares of velocity innovations
|
||||
float varVelSum = 0; // sum of velocity innovation variances
|
||||
for (uint8_t i = 0; i<=imax; i++) {
|
||||
// velocity states start at index 4
|
||||
stateIndex = i + 4;
|
||||
// calculate innovations using blended and single IMU predicted states
|
||||
velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended
|
||||
// calculate innovation variance
|
||||
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
|
||||
// sum the innovation and innovation variances
|
||||
innovVelSumSq += sq(velInnov[i]);
|
||||
varVelSum += varInnovVelPos[i];
|
||||
}
|
||||
// apply an innovation consistency threshold test, but don't fail if bad IMU data
|
||||
// calculate the test ratio
|
||||
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
|
||||
// fail if the ratio is greater than 1
|
||||
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
|
||||
// use velocity data if healthy, timed out, or in constant position mode
|
||||
if (velHealth || velTimeout) {
|
||||
velHealth = true;
|
||||
// restart the timeout count
|
||||
lastVelPassTime_ms = imuSampleTime_ms;
|
||||
// If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
|
||||
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
|
||||
// reset the velocity to the GPS velocity
|
||||
ResetVelocity();
|
||||
// don't fuse GPS velocity data on this time step
|
||||
fuseVelData = false;
|
||||
// Reset the normalised innovation to avoid failing the bad fusion tests
|
||||
velTestRatio = 0.0f;
|
||||
}
|
||||
} else {
|
||||
velHealth = false;
|
||||
}
|
||||
}
|
||||
|
||||
// test height measurements
|
||||
if (fuseHgtData) {
|
||||
// calculate height innovations
|
||||
innovVelPos[5] = stateStruct.position.z - observation[5];
|
||||
varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
|
||||
// calculate the innovation consistency test ratio
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
|
||||
// Fuse height data if healthy or timed out or in constant position mode
|
||||
if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) {
|
||||
// Calculate a filtered value to be used by pre-flight health checks
|
||||
// We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
|
||||
if (onGround) {
|
||||
float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
|
||||
const float hgtInnovFiltTC = 2.0f;
|
||||
float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
|
||||
hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
|
||||
} else {
|
||||
hgtInnovFiltState = 0.0f;
|
||||
}
|
||||
|
||||
// if timed out, reset the height
|
||||
if (hgtTimeout) {
|
||||
ResetHeight();
|
||||
}
|
||||
|
||||
// If we have got this far then declare the height data as healthy and reset the timeout counter
|
||||
hgtHealth = true;
|
||||
lastHgtPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
}
|
||||
|
||||
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
|
||||
if (fuseVelData && velHealth) {
|
||||
fuseData[0] = true;
|
||||
fuseData[1] = true;
|
||||
if (useGpsVertVel) {
|
||||
fuseData[2] = true;
|
||||
}
|
||||
}
|
||||
if (fusePosData && posHealth) {
|
||||
fuseData[3] = true;
|
||||
fuseData[4] = true;
|
||||
}
|
||||
if (fuseHgtData && hgtHealth) {
|
||||
fuseData[5] = true;
|
||||
}
|
||||
|
||||
// fuse measurements sequentially
|
||||
for (obsIndex=0; obsIndex<=5; obsIndex++) {
|
||||
if (fuseData[obsIndex]) {
|
||||
stateIndex = 4 + obsIndex;
|
||||
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
|
||||
// adjust scaling on GPS measurement noise variances if not enough satellites
|
||||
if (obsIndex <= 2)
|
||||
{
|
||||
innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex];
|
||||
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
||||
}
|
||||
else if (obsIndex == 3 || obsIndex == 4) {
|
||||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||||
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
|
||||
} else if (obsIndex == 5) {
|
||||
innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex];
|
||||
const float gndMaxBaroErr = 4.0f;
|
||||
const float gndBaroInnovFloor = -0.5f;
|
||||
|
||||
if(getTouchdownExpected()) {
|
||||
// when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
|
||||
// constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
|
||||
// this function looks like this:
|
||||
// |/
|
||||
//---------|---------
|
||||
// ____/|
|
||||
// / |
|
||||
// / |
|
||||
innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the Kalman gain and calculate innovation variances
|
||||
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
|
||||
SK = 1.0f/varInnovVelPos[obsIndex];
|
||||
for (uint8_t i= 0; i<=15; i++) {
|
||||
Kfusion[i] = P[i][stateIndex]*SK;
|
||||
}
|
||||
|
||||
// inhibit magnetic field state estimation by setting Kalman gains to zero
|
||||
if (!inhibitMagStates) {
|
||||
for (uint8_t i = 16; i<=21; i++) {
|
||||
Kfusion[i] = P[i][stateIndex]*SK;
|
||||
}
|
||||
} else {
|
||||
for (uint8_t i = 16; i<=21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// inhibit wind state estimation by setting Kalman gains to zero
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[22] = P[22][stateIndex]*SK;
|
||||
Kfusion[23] = P[23][stateIndex]*SK;
|
||||
} else {
|
||||
Kfusion[22] = 0.0f;
|
||||
Kfusion[23] = 0.0f;
|
||||
}
|
||||
|
||||
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
|
||||
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++)
|
||||
{
|
||||
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
|
||||
}
|
||||
}
|
||||
// Check that we are not going to drive any variances negative and skip the update if so
|
||||
bool healthyFusion = true;
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
if (KHP[i][i] > P[i][i]) {
|
||||
healthyFusion = false;
|
||||
}
|
||||
}
|
||||
if (healthyFusion) {
|
||||
// update the covariance matrix
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// update states and renormalise the quaternions
|
||||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||||
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
|
||||
}
|
||||
stateStruct.quat.normalize();
|
||||
|
||||
// record good fusion status
|
||||
if (obsIndex == 0) {
|
||||
faultStatus.bad_nvel = false;
|
||||
} else if (obsIndex == 1) {
|
||||
faultStatus.bad_evel = false;
|
||||
} else if (obsIndex == 2) {
|
||||
faultStatus.bad_dvel = false;
|
||||
} else if (obsIndex == 3) {
|
||||
faultStatus.bad_npos = false;
|
||||
} else if (obsIndex == 4) {
|
||||
faultStatus.bad_epos = false;
|
||||
} else if (obsIndex == 5) {
|
||||
faultStatus.bad_dpos = false;
|
||||
}
|
||||
} else {
|
||||
// record bad fusion status
|
||||
if (obsIndex == 0) {
|
||||
faultStatus.bad_nvel = true;
|
||||
} else if (obsIndex == 1) {
|
||||
faultStatus.bad_evel = true;
|
||||
} else if (obsIndex == 2) {
|
||||
faultStatus.bad_dvel = true;
|
||||
} else if (obsIndex == 3) {
|
||||
faultStatus.bad_npos = true;
|
||||
} else if (obsIndex == 4) {
|
||||
faultStatus.bad_epos = true;
|
||||
} else if (obsIndex == 5) {
|
||||
faultStatus.bad_dpos = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// stop performance timer
|
||||
hal.util->perf_end(_perf_FuseVelPosNED);
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
* MISC FUNCTIONS *
|
||||
********************************************************/
|
||||
|
||||
// select the height measurement to be fused from the available baro, range finder and GPS sources
|
||||
void NavEKF3_core::selectHeightForFusion()
|
||||
{
|
||||
// Read range finder data and check for new data in the buffer
|
||||
// This data is used by both height and optical flow fusion processing
|
||||
readRangeFinder();
|
||||
rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
|
||||
|
||||
// correct range data for the body frame position offset relative to the IMU
|
||||
// the corrected reading is the reading that would have been taken if the sensor was
|
||||
// co-located with the IMU
|
||||
if (rangeDataToFuse) {
|
||||
Vector3f posOffsetBody = frontend->_rng.get_pos_offset(rangeDataDelayed.sensor_idx) - accelPosOffset;
|
||||
if (!posOffsetBody.is_zero()) {
|
||||
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
|
||||
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
|
||||
}
|
||||
}
|
||||
|
||||
// read baro height data from the sensor and check for new data in the buffer
|
||||
readBaroData();
|
||||
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
||||
|
||||
// select height source
|
||||
if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
|
||||
if (frontend->_altSource == 1) {
|
||||
// always use range finder
|
||||
activeHgtSource = HGT_SOURCE_RNG;
|
||||
} else {
|
||||
// determine if we are above or below the height switch region
|
||||
float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm() * (float)frontend->_useRngSwHgt;
|
||||
bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
|
||||
bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
|
||||
|
||||
// If the terrain height is consistent and we are moving slowly, then it can be
|
||||
// used as a height reference in combination with a range finder
|
||||
float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
|
||||
bool dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable;
|
||||
bool trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable;
|
||||
|
||||
/*
|
||||
* Switch between range finder and primary height source using height above ground and speed thresholds with
|
||||
* hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
|
||||
* which cannot be assumed if the vehicle is moving horizontally.
|
||||
*/
|
||||
if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
|
||||
// cannot trust terrain or range finder so stop using range finder height
|
||||
if (frontend->_altSource == 0) {
|
||||
activeHgtSource = HGT_SOURCE_BARO;
|
||||
} else if (frontend->_altSource == 2) {
|
||||
activeHgtSource = HGT_SOURCE_GPS;
|
||||
}
|
||||
} else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) {
|
||||
// reliable terrain and range finder so start using range finder height
|
||||
activeHgtSource = HGT_SOURCE_RNG;
|
||||
}
|
||||
}
|
||||
} else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
|
||||
activeHgtSource = HGT_SOURCE_GPS;
|
||||
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
|
||||
activeHgtSource = HGT_SOURCE_BCN;
|
||||
} else {
|
||||
activeHgtSource = HGT_SOURCE_BARO;
|
||||
}
|
||||
|
||||
// Use Baro alt as a fallback if we lose range finder or GPS
|
||||
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500));
|
||||
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
|
||||
if (lostRngHgt || lostGpsHgt) {
|
||||
activeHgtSource = HGT_SOURCE_BARO;
|
||||
}
|
||||
|
||||
// if there is new baro data to fuse, calculate filtered baro data required by other processes
|
||||
if (baroDataToFuse) {
|
||||
// calculate offset to baro data that enables us to switch to Baro height use during operation
|
||||
if (activeHgtSource != HGT_SOURCE_BARO) {
|
||||
calcFiltBaroOffset();
|
||||
}
|
||||
// filtered baro data used to provide a reference for takeoff
|
||||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||||
if (!getTakeoffExpected()) {
|
||||
const float gndHgtFiltTC = 0.5f;
|
||||
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
|
||||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||||
}
|
||||
}
|
||||
|
||||
// calculate offset to GPS height data that enables us to switch to GPS height during operation
|
||||
if (gpsDataToFuse && (activeHgtSource != HGT_SOURCE_GPS)) {
|
||||
calcFiltGpsHgtOffset();
|
||||
}
|
||||
|
||||
// Select the height measurement source
|
||||
if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
|
||||
// using range finder data
|
||||
// correct for tilt using a flat earth model
|
||||
if (prevTnb.c.z >= 0.7) {
|
||||
// calculate height above ground
|
||||
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
|
||||
// correct for terrain position relative to datum
|
||||
hgtMea -= terrainState;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise
|
||||
posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
|
||||
// add uncertainty created by terrain gradient and vehicle tilt
|
||||
posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
|
||||
} else {
|
||||
// disable fusion if tilted too far
|
||||
fuseHgtData = false;
|
||||
}
|
||||
} else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
|
||||
// using GPS data
|
||||
hgtMea = gpsDataDelayed.hgt;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
|
||||
if (gpsHgtAccuracy > 0.0f) {
|
||||
posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
|
||||
} else {
|
||||
posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
|
||||
}
|
||||
} else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
|
||||
// using Baro data
|
||||
hgtMea = baroDataDelayed.hgt - baroHgtOffset;
|
||||
// enable fusion
|
||||
fuseHgtData = true;
|
||||
// set the observation noise
|
||||
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
|
||||
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
|
||||
if (getTakeoffExpected() || getTouchdownExpected()) {
|
||||
posDownObsNoise *= frontend->gndEffectBaroScaler;
|
||||
}
|
||||
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
|
||||
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
|
||||
if (motorsArmed && getTakeoffExpected()) {
|
||||
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
|
||||
}
|
||||
} else {
|
||||
fuseHgtData = false;
|
||||
}
|
||||
|
||||
// If we haven't fused height data for a while, then declare the height data as being timed out
|
||||
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
|
||||
hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
|
||||
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
|
||||
hgtTimeout = true;
|
||||
} else {
|
||||
hgtTimeout = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
583
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
Normal file
583
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
Normal file
@ -0,0 +1,583 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/********************************************************
|
||||
* FUSE MEASURED_DATA *
|
||||
********************************************************/
|
||||
|
||||
// select fusion of range beacon measurements
|
||||
void NavEKF3_core::SelectRngBcnFusion()
|
||||
{
|
||||
// read range data from the sensor and check for new data in the buffer
|
||||
readRngBcnData();
|
||||
|
||||
// Determine if we need to fuse range beacon data on this time step
|
||||
if (rngBcnDataToFuse) {
|
||||
if (PV_AidingMode == AID_ABSOLUTE) {
|
||||
// Normal operating mode is to fuse the range data into the main filter
|
||||
FuseRngBcn();
|
||||
} else {
|
||||
// If we aren't able to use the data in the main filter, use a simple 3-state filter to estimte position only
|
||||
FuseRngBcnStatic();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavEKF3_core::FuseRngBcn()
|
||||
{
|
||||
// declarations
|
||||
float pn;
|
||||
float pe;
|
||||
float pd;
|
||||
float bcn_pn;
|
||||
float bcn_pe;
|
||||
float bcn_pd;
|
||||
const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
float rngPred;
|
||||
|
||||
// health is set bad until test passed
|
||||
rngBcnHealth = false;
|
||||
|
||||
if (activeHgtSource != HGT_SOURCE_BCN) {
|
||||
// calculate the vertical offset from EKF datum to beacon datum
|
||||
CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
|
||||
} else {
|
||||
bcnPosOffset = 0.0f;
|
||||
}
|
||||
|
||||
// copy required states to local variable names
|
||||
pn = stateStruct.position.x;
|
||||
pe = stateStruct.position.y;
|
||||
pd = stateStruct.position.z;
|
||||
bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
|
||||
bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
|
||||
bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset;
|
||||
|
||||
// predicted range
|
||||
Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
rngPred = deltaPosNED.length();
|
||||
|
||||
// calculate measurement innovation
|
||||
innovRngBcn = rngPred - rngBcnDataDelayed.rng;
|
||||
|
||||
// perform fusion of range measurement
|
||||
if (rngPred > 0.1f)
|
||||
{
|
||||
// calculate observation jacobians
|
||||
float H_BCN[24];
|
||||
memset(H_BCN, 0, sizeof(H_BCN));
|
||||
float t2 = bcn_pd-pd;
|
||||
float t3 = bcn_pe-pe;
|
||||
float t4 = bcn_pn-pn;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
float t9 = 1.0f/sqrtf(t8);
|
||||
H_BCN[7] = -t4*t9;
|
||||
H_BCN[8] = -t3*t9;
|
||||
// If we are not using the beacons as a height reference, we pretend that the beacons
|
||||
// are a the same height as the flight vehicle when calculating the observation derivatives
|
||||
// and Kalman gains
|
||||
// TODO - less hacky way of achieving this, preferably using an alternative derivation
|
||||
if (activeHgtSource != HGT_SOURCE_BCN) {
|
||||
t2 = 0.0f;
|
||||
}
|
||||
H_BCN[9] = -t2*t9;
|
||||
|
||||
// calculate Kalman gains
|
||||
float t10 = P[9][9]*t2*t9;
|
||||
float t11 = P[8][9]*t3*t9;
|
||||
float t12 = P[7][9]*t4*t9;
|
||||
float t13 = t10+t11+t12;
|
||||
float t14 = t2*t9*t13;
|
||||
float t15 = P[9][8]*t2*t9;
|
||||
float t16 = P[8][8]*t3*t9;
|
||||
float t17 = P[7][8]*t4*t9;
|
||||
float t18 = t15+t16+t17;
|
||||
float t19 = t3*t9*t18;
|
||||
float t20 = P[9][7]*t2*t9;
|
||||
float t21 = P[8][7]*t3*t9;
|
||||
float t22 = P[7][7]*t4*t9;
|
||||
float t23 = t20+t21+t22;
|
||||
float t24 = t4*t9*t23;
|
||||
varInnovRngBcn = R_BCN+t14+t19+t24;
|
||||
float t26;
|
||||
if (varInnovRngBcn >= R_BCN) {
|
||||
t26 = 1.0f/varInnovRngBcn;
|
||||
faultStatus.bad_rngbcn = 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_rngbcn = true;
|
||||
return;
|
||||
}
|
||||
|
||||
Kfusion[0] = -t26*(P[0][7]*t4*t9+P[0][8]*t3*t9+P[0][9]*t2*t9);
|
||||
Kfusion[1] = -t26*(P[1][7]*t4*t9+P[1][8]*t3*t9+P[1][9]*t2*t9);
|
||||
Kfusion[2] = -t26*(P[2][7]*t4*t9+P[2][8]*t3*t9+P[2][9]*t2*t9);
|
||||
Kfusion[3] = -t26*(P[3][7]*t4*t9+P[3][8]*t3*t9+P[3][9]*t2*t9);
|
||||
Kfusion[4] = -t26*(P[4][7]*t4*t9+P[4][8]*t3*t9+P[4][9]*t2*t9);
|
||||
Kfusion[5] = -t26*(P[5][7]*t4*t9+P[5][8]*t3*t9+P[5][9]*t2*t9);
|
||||
Kfusion[7] = -t26*(t22+P[7][8]*t3*t9+P[7][9]*t2*t9);
|
||||
Kfusion[8] = -t26*(t16+P[8][7]*t4*t9+P[8][9]*t2*t9);
|
||||
Kfusion[10] = -t26*(P[10][7]*t4*t9+P[10][8]*t3*t9+P[10][9]*t2*t9);
|
||||
Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9);
|
||||
Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9);
|
||||
Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9);
|
||||
Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9);
|
||||
Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9);
|
||||
// only allow the range observations to modify the vertical states if we are using it as a height reference
|
||||
if (activeHgtSource == HGT_SOURCE_BCN) {
|
||||
Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9);
|
||||
Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9);
|
||||
} else {
|
||||
Kfusion[6] = 0.0f;
|
||||
Kfusion[9] = 0.0f;
|
||||
}
|
||||
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = -t26*(P[16][7]*t4*t9+P[16][8]*t3*t9+P[16][9]*t2*t9);
|
||||
Kfusion[17] = -t26*(P[17][7]*t4*t9+P[17][8]*t3*t9+P[17][9]*t2*t9);
|
||||
Kfusion[18] = -t26*(P[18][7]*t4*t9+P[18][8]*t3*t9+P[18][9]*t2*t9);
|
||||
Kfusion[19] = -t26*(P[19][7]*t4*t9+P[19][8]*t3*t9+P[19][9]*t2*t9);
|
||||
Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9);
|
||||
Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9);
|
||||
} else {
|
||||
// zero indexes 16 to 21 = 6*4 bytes
|
||||
memset(&Kfusion[16], 0, 24);
|
||||
}
|
||||
Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
|
||||
Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
|
||||
|
||||
// Calculate innovation using the selected offset value
|
||||
Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
|
||||
innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
|
||||
|
||||
// fail if the ratio is > 1, but don't fail if bad IMU data
|
||||
rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
|
||||
|
||||
// test the ratio before fusing data
|
||||
if (rngBcnHealth) {
|
||||
|
||||
// restart the counter
|
||||
lastRngBcnPassTime_ms = imuSampleTime_ms;
|
||||
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||
for (unsigned j = 0; j<=6; j++) {
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
for (unsigned j = 7; j<=9; j++) {
|
||||
KH[i][j] = Kfusion[i] * H_BCN[j];
|
||||
}
|
||||
for (unsigned j = 10; j<=23; j++) {
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
}
|
||||
for (unsigned j = 0; j<=stateIndexLim; j++) {
|
||||
for (unsigned i = 0; i<=stateIndexLim; i++) {
|
||||
ftype res = 0;
|
||||
res += KH[i][7] * P[7][j];
|
||||
res += KH[i][8] * P[8][j];
|
||||
res += KH[i][9] * P[9][j];
|
||||
KHP[i][j] = res;
|
||||
}
|
||||
}
|
||||
// Check that we are not going to drive any variances negative and skip the update if so
|
||||
bool healthyFusion = true;
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
if (KHP[i][i] > P[i][i]) {
|
||||
healthyFusion = false;
|
||||
}
|
||||
}
|
||||
if (healthyFusion) {
|
||||
// update the covariance matrix
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) {
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j<=stateIndexLim; j++) {
|
||||
statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn;
|
||||
}
|
||||
|
||||
// record healthy fusion
|
||||
faultStatus.bad_rngbcn = false;
|
||||
|
||||
} else {
|
||||
// record bad fusion
|
||||
faultStatus.bad_rngbcn = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Update the fusion report
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
|
||||
rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Use range beaon measurements to calculate a static position using a 3-state EKF algorithm.
|
||||
Algorihtm based on the following:
|
||||
https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon.m
|
||||
*/
|
||||
void NavEKF3_core::FuseRngBcnStatic()
|
||||
{
|
||||
// get the estimated range measurement variance
|
||||
const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
|
||||
|
||||
/*
|
||||
The first thing to do is to check if we have started the alignment and if not, initialise the
|
||||
states and covariance to a first guess. To do this iterate through the avilable beacons and then
|
||||
initialise the initial position to the mean beacon position. The initial position uncertainty
|
||||
is set to the mean range measurement.
|
||||
*/
|
||||
if (!rngBcnAlignmentStarted) {
|
||||
if (rngBcnDataDelayed.beacon_ID != lastBeaconIndex) {
|
||||
rngBcnPosSum += rngBcnDataDelayed.beacon_posNED;
|
||||
lastBeaconIndex = rngBcnDataDelayed.beacon_ID;
|
||||
rngSum += rngBcnDataDelayed.rng;
|
||||
numBcnMeas++;
|
||||
|
||||
// capture the beacon vertical spread
|
||||
if (rngBcnDataDelayed.beacon_posNED.z > maxBcnPosD) {
|
||||
maxBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
|
||||
} else if(rngBcnDataDelayed.beacon_posNED.z < minBcnPosD) {
|
||||
minBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
|
||||
}
|
||||
}
|
||||
if (numBcnMeas >= 100) {
|
||||
rngBcnAlignmentStarted = true;
|
||||
float tempVar = 1.0f / (float)numBcnMeas;
|
||||
// initialise the receiver position to the centre of the beacons and at zero height
|
||||
receiverPos.x = rngBcnPosSum.x * tempVar;
|
||||
receiverPos.y = rngBcnPosSum.y * tempVar;
|
||||
receiverPos.z = 0.0f;
|
||||
receiverPosCov[2][2] = receiverPosCov[1][1] = receiverPosCov[0][0] = rngSum * tempVar;
|
||||
lastBeaconIndex = 0;
|
||||
numBcnMeas = 0;
|
||||
rngBcnPosSum.zero();
|
||||
rngSum = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
if (rngBcnAlignmentStarted) {
|
||||
numBcnMeas++;
|
||||
|
||||
if (numBcnMeas >= 100) {
|
||||
// 100 observations is enough for a stable estimate under most conditions
|
||||
// TODO monitor stability of the position estimate
|
||||
rngBcnAlignmentCompleted = true;
|
||||
|
||||
}
|
||||
|
||||
if (rngBcnAlignmentCompleted) {
|
||||
if (activeHgtSource != HGT_SOURCE_BCN) {
|
||||
// We are using a different height reference for the main EKF so need to estimate a vertical
|
||||
// position offset to be applied to the beacon system that minimises the range innovations
|
||||
// The position estimate should be stable after 100 iterations so we use a simple dual
|
||||
// hypothesis 1-state EKF to estiate the offset
|
||||
Vector3f refPosNED;
|
||||
refPosNED.x = receiverPos.x;
|
||||
refPosNED.y = receiverPos.y;
|
||||
refPosNED.z = stateStruct.position.z;
|
||||
CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true);
|
||||
|
||||
} else {
|
||||
// we are using the beacons as the primary height source, so don't modify their vertical position
|
||||
bcnPosOffset = 0.0f;
|
||||
|
||||
}
|
||||
} else {
|
||||
if (activeHgtSource != HGT_SOURCE_BCN) {
|
||||
// The position estimate is not yet stable so we cannot run the 1-state EKF to estimate
|
||||
// beacon system vertical position offset. Instead we initialise the dual hypothesis offset states
|
||||
// using the beacon vertical position, vertical position estimate relative to the beacon origin
|
||||
// and the main EKF vertical position
|
||||
|
||||
// Calculate the mid vertical position of all beacons
|
||||
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
|
||||
// calculate the delta to the estimated receiver position
|
||||
float delta = receiverPos.z - bcnMidPosD;
|
||||
|
||||
// calcuate the two hypothesis for our vertical position
|
||||
float receverPosDownMax;
|
||||
float receverPosDownMin;
|
||||
if (delta >= 0.0f) {
|
||||
receverPosDownMax = receiverPos.z;
|
||||
receverPosDownMin = receiverPos.z - 2.0f * delta;
|
||||
} else {
|
||||
receverPosDownMax = receiverPos.z - 2.0f * delta;
|
||||
receverPosDownMin = receiverPos.z;
|
||||
}
|
||||
|
||||
bcnPosDownOffsetMax = stateStruct.position.z - receverPosDownMin;
|
||||
bcnPosDownOffsetMin = stateStruct.position.z - receverPosDownMax;
|
||||
} else {
|
||||
// We are using the beacons as the primary height reference, so don't modify their vertical position
|
||||
bcnPosOffset = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// Add some process noise to the states at each time step
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
receiverPosCov[i][i] += 0.1f;
|
||||
}
|
||||
|
||||
// calculate the observation jacobian
|
||||
float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset;
|
||||
float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
|
||||
float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
if (t8 < 0.1f) {
|
||||
// calculation will be badly conditioned
|
||||
return;
|
||||
}
|
||||
float t9 = 1.0f/sqrtf(t8);
|
||||
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
|
||||
float t15 = receiverPos.x*2.0f;
|
||||
float t11 = t10-t15;
|
||||
float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
|
||||
float t14 = receiverPos.y*2.0f;
|
||||
float t13 = t12-t14;
|
||||
float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
|
||||
float t18 = receiverPos.z*2.0f;
|
||||
float t17 = t16-t18;
|
||||
float H_RNG[3];
|
||||
H_RNG[0] = -t9*t11*0.5f;
|
||||
H_RNG[1] = -t9*t13*0.5f;
|
||||
H_RNG[2] = -t9*t17*0.5f;
|
||||
|
||||
// calculate the Kalman gains
|
||||
float t19 = receiverPosCov[0][0]*t9*t11*0.5f;
|
||||
float t20 = receiverPosCov[1][1]*t9*t13*0.5f;
|
||||
float t21 = receiverPosCov[0][1]*t9*t11*0.5f;
|
||||
float t22 = receiverPosCov[2][1]*t9*t17*0.5f;
|
||||
float t23 = t20+t21+t22;
|
||||
float t24 = t9*t13*t23*0.5f;
|
||||
float t25 = receiverPosCov[1][2]*t9*t13*0.5f;
|
||||
float t26 = receiverPosCov[0][2]*t9*t11*0.5f;
|
||||
float t27 = receiverPosCov[2][2]*t9*t17*0.5f;
|
||||
float t28 = t25+t26+t27;
|
||||
float t29 = t9*t17*t28*0.5f;
|
||||
float t30 = receiverPosCov[1][0]*t9*t13*0.5f;
|
||||
float t31 = receiverPosCov[2][0]*t9*t17*0.5f;
|
||||
float t32 = t19+t30+t31;
|
||||
float t33 = t9*t11*t32*0.5f;
|
||||
varInnovRngBcn = R_RNG+t24+t29+t33;
|
||||
float t35 = 1.0f/varInnovRngBcn;
|
||||
float K_RNG[3];
|
||||
K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
|
||||
K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
|
||||
K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
|
||||
|
||||
// calculate range measurement innovation
|
||||
Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
|
||||
deltaPosNED.z -= bcnPosOffset;
|
||||
innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
|
||||
|
||||
// update the state
|
||||
receiverPos.x -= K_RNG[0] * innovRngBcn;
|
||||
receiverPos.y -= K_RNG[1] * innovRngBcn;
|
||||
receiverPos.z -= K_RNG[2] * innovRngBcn;
|
||||
receiverPos.z = MAX(receiverPos.z, minBcnPosD + 1.2f);
|
||||
|
||||
// calculate the covariance correction
|
||||
for (unsigned i = 0; i<=2; i++) {
|
||||
for (unsigned j = 0; j<=2; j++) {
|
||||
KH[i][j] = K_RNG[i] * H_RNG[j];
|
||||
}
|
||||
}
|
||||
for (unsigned j = 0; j<=2; j++) {
|
||||
for (unsigned i = 0; i<=2; i++) {
|
||||
ftype res = 0;
|
||||
res += KH[i][0] * receiverPosCov[0][j];
|
||||
res += KH[i][1] * receiverPosCov[1][j];
|
||||
res += KH[i][2] * receiverPosCov[2][j];
|
||||
KHP[i][j] = res;
|
||||
}
|
||||
}
|
||||
// prevent negative variances
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
if (receiverPosCov[i][i] < 0.0f) {
|
||||
receiverPosCov[i][i] = 0.0f;
|
||||
KHP[i][i] = 0.0f;
|
||||
} else if (KHP[i][i] > receiverPosCov[i][i]) {
|
||||
KHP[i][i] = receiverPosCov[i][i];
|
||||
}
|
||||
}
|
||||
// apply the covariance correction
|
||||
for (uint8_t i= 0; i<=2; i++) {
|
||||
for (uint8_t j= 0; j<=2; j++) {
|
||||
receiverPosCov[i][j] -= KHP[i][j];
|
||||
}
|
||||
}
|
||||
// ensure the covariance matrix is symmetric
|
||||
for (uint8_t i=1; i<=2; i++) {
|
||||
for (uint8_t j=0; j<=i-1; j++) {
|
||||
float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
|
||||
receiverPosCov[i][j] = temp;
|
||||
receiverPosCov[j][i] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
if (numBcnMeas >= 100) {
|
||||
// 100 observations is enough for a stable estimate under most conditions
|
||||
// TODO monitor stability of the position estimate
|
||||
rngBcnAlignmentCompleted = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
|
||||
Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
|
||||
*/
|
||||
void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning)
|
||||
{
|
||||
// Handle height offsets between the primary height source and the range beacons by estimating
|
||||
// the beacon systems global vertical position offset using a single state Kalman filter
|
||||
// The estimated offset is used to correct the beacon height when calculating innovations
|
||||
// A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
|
||||
// The main filter then uses the offset with the smaller innovations
|
||||
|
||||
float innov; // range measurement innovation (m)
|
||||
float innovVar; // range measurement innovation variance (m^2)
|
||||
float gain; // Kalman gain
|
||||
float obsDeriv; // derivative of observation relative to state
|
||||
|
||||
const float stateNoiseVar = 0.1f; // State process noise variance
|
||||
const float filtAlpha = 0.01f; // LPF constant
|
||||
const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
|
||||
|
||||
// estimate upper value for offset
|
||||
|
||||
// calculate observation derivative
|
||||
float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax;
|
||||
float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
|
||||
float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
|
||||
float t5 = t2*t2;
|
||||
float t6 = t3*t3;
|
||||
float t7 = t4*t4;
|
||||
float t8 = t5+t6+t7;
|
||||
float t9;
|
||||
if (t8 > 0.1f) {
|
||||
t9 = 1.0f/sqrtf(t8);
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
|
||||
|
||||
// calculate a filtered innovation magnitude to be used to select between the high or low offset
|
||||
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov);
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMaxVar += stateNoiseVar;
|
||||
|
||||
// calculate the innovation variance
|
||||
innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
|
||||
innovVar = MAX(innovVar, obsVar);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
// calculate the Kalman gain
|
||||
gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
|
||||
|
||||
// state update
|
||||
bcnPosDownOffsetMax -= innov * gain;
|
||||
|
||||
// covariance update
|
||||
bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar;
|
||||
bcnPosOffsetMaxVar = MAX(bcnPosOffsetMaxVar, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// estimate lower value for offset
|
||||
|
||||
// calculate observation derivative
|
||||
t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMin;
|
||||
t5 = t2*t2;
|
||||
t8 = t5+t6+t7;
|
||||
if (t8 > 0.1f) {
|
||||
t9 = 1.0f/sqrtf(t8);
|
||||
obsDeriv = t2*t9;
|
||||
|
||||
// Calculate innovation
|
||||
innov = sqrtf(t8) - rngBcnDataDelayed.rng;
|
||||
|
||||
// calculate a filtered innovation magnitude to be used to select between the high or low offset
|
||||
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov);
|
||||
|
||||
// covariance prediction
|
||||
bcnPosOffsetMinVar += stateNoiseVar;
|
||||
|
||||
// calculate the innovation variance
|
||||
innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
|
||||
innovVar = MAX(innovVar, obsVar);
|
||||
|
||||
// Reject range innovation spikes using a 5-sigma threshold unless aligning
|
||||
if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
|
||||
// calculate the Kalman gain
|
||||
gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
|
||||
|
||||
// state update
|
||||
bcnPosDownOffsetMin -= innov * gain;
|
||||
|
||||
// covariance update
|
||||
bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
|
||||
bcnPosOffsetMinVar = MAX(bcnPosOffsetMinVar, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the mid vertical position of all beacons
|
||||
float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
|
||||
|
||||
// ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
|
||||
bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
|
||||
bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f);
|
||||
|
||||
// calculate the innovation for the main filter using the offset with the smallest innovation history
|
||||
// apply hysteresis to prevent rapid switching
|
||||
if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) {
|
||||
bcnPosOffset = bcnPosDownOffsetMin;
|
||||
usingMinHypothesis = true;
|
||||
} else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) {
|
||||
bcnPosOffset = bcnPosDownOffsetMax;
|
||||
usingMinHypothesis = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
449
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
Normal file
449
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
Normal file
@ -0,0 +1,449 @@
|
||||
/// -*- 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
|
||||
|
||||
#include "AP_NavEKF3.h"
|
||||
#include "AP_NavEKF3_core.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.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 NavEKF3_core::calcGpsGoodToAlign(void)
|
||||
{
|
||||
if (inFlight && assume_zero_sideslip() && !use_compass()) {
|
||||
// this is a special case where a plane has launched without magnetometer
|
||||
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
|
||||
return true;
|
||||
}
|
||||
|
||||
// User defined multiplier to be applied to check thresholds
|
||||
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
|
||||
|
||||
// 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 && yawTestRatio <= 1.0f) || !consistentMagData) {
|
||||
magYawResetTimer_ms = imuSampleTime_ms;
|
||||
}
|
||||
if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !motorsArmed) {
|
||||
// request reset of heading and magnetic field states
|
||||
magYawResetRequest = true;
|
||||
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
|
||||
magYawResetTimer_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// Check for significant change in GPS position if disarmed which indicates bad GPS
|
||||
// This check can only be used when the vehicle is stationary
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
|
||||
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
|
||||
// calculate time lapsesd since last update and limit to prevent numerical errors
|
||||
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
|
||||
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
|
||||
// Sum distance moved
|
||||
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
|
||||
gpsloc_prev = gpsloc;
|
||||
// Decay distance moved exponentially to zero
|
||||
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
|
||||
// Clamp the fiter state to prevent excessive persistence of large transients
|
||||
gpsDriftNE = MIN(gpsDriftNE,10.0f);
|
||||
// Fail if more than 3 metres drift after filtering whilst on-ground
|
||||
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
|
||||
bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsDriftFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
|
||||
gpsCheckStatus.bad_horiz_drift = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_horiz_drift = false;
|
||||
}
|
||||
|
||||
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
|
||||
bool gpsVertVelFail;
|
||||
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
|
||||
// check that the average vertical GPS velocity is close to zero
|
||||
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
|
||||
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
|
||||
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
|
||||
} else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
|
||||
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
|
||||
gpsVertVelFail = true;
|
||||
// if we have a 3D fix with no vertical velocity and
|
||||
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
|
||||
// capable of giving a vertical velocity
|
||||
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
frontend->_fusionModeGPS.set(1);
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
|
||||
}
|
||||
} else {
|
||||
gpsVertVelFail = false;
|
||||
}
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsVertVelFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
|
||||
gpsCheckStatus.bad_vert_vel = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_vert_vel = false;
|
||||
}
|
||||
|
||||
// Check that the horizontal GPS vertical velocity is reasonable after noise filtering
|
||||
// This check can only be used if the vehicle is stationary
|
||||
bool gpsHorizVelFail;
|
||||
if (onGround) {
|
||||
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
|
||||
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
|
||||
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
|
||||
} else {
|
||||
gpsHorizVelFail = false;
|
||||
}
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsHorizVelFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
|
||||
gpsCheckStatus.bad_horiz_vel = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_horiz_vel = false;
|
||||
}
|
||||
|
||||
// fail if horiziontal position accuracy not sufficient
|
||||
float hAcc = 0.0f;
|
||||
bool hAccFail;
|
||||
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
|
||||
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
|
||||
} else {
|
||||
hAccFail = false;
|
||||
}
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (hAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
|
||||
gpsCheckStatus.bad_hAcc = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_hAcc = false;
|
||||
}
|
||||
|
||||
// fail if reported speed accuracy greater than threshold
|
||||
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (gpsSpdAccFail) {
|
||||
hal.util->snprintf(prearm_fail_string,
|
||||
sizeof(prearm_fail_string),
|
||||
"GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
|
||||
gpsCheckStatus.bad_sAcc = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_sAcc = false;
|
||||
}
|
||||
|
||||
// fail if satellite geometry is poor
|
||||
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (hdopFail) {
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
|
||||
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
|
||||
gpsCheckStatus.bad_hdop = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_hdop = false;
|
||||
}
|
||||
|
||||
// fail if not enough sats
|
||||
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
if (numSatsFail) {
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
|
||||
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
|
||||
gpsCheckStatus.bad_sats = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_sats = false;
|
||||
}
|
||||
|
||||
// fail if magnetometer innovations are outside limits indicating bad yaw
|
||||
// with bad yaw we are unable to use GPS
|
||||
bool yawFail;
|
||||
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f || yawTestRatio > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
|
||||
yawFail = true;
|
||||
} else {
|
||||
yawFail = false;
|
||||
}
|
||||
|
||||
// Report check result as a text string and bitmask
|
||||
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);
|
||||
gpsCheckStatus.bad_yaw = true;
|
||||
} else {
|
||||
gpsCheckStatus.bad_yaw = false;
|
||||
}
|
||||
|
||||
// assume failed first time through and notify user checks have started
|
||||
if (lastGpsVelFail_ms == 0) {
|
||||
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
|
||||
lastGpsVelFail_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// record time of fail
|
||||
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
|
||||
lastGpsVelFail_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// continuous period without fail required to return a healthy status
|
||||
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
|
||||
void NavEKF3_core::calcGpsGoodForFlight(void)
|
||||
{
|
||||
// use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
|
||||
|
||||
// set up varaibles and constants used by filter that is applied to GPS speed accuracy
|
||||
const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
|
||||
const float tau = 10.0f; // time constant (sec) of peak hold decay
|
||||
if (lastGpsCheckTime_ms == 0) {
|
||||
lastGpsCheckTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
|
||||
lastGpsCheckTime_ms = imuSampleTime_ms;
|
||||
float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f);
|
||||
|
||||
// get the receivers reported speed accuracy
|
||||
float gpsSpdAccRaw;
|
||||
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
|
||||
gpsSpdAccRaw = 0.0f;
|
||||
}
|
||||
|
||||
// filter the raw speed accuracy using a LPF
|
||||
sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
|
||||
|
||||
// apply a peak hold filter to the LPF output
|
||||
sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
|
||||
|
||||
// Apply a threshold test with hysteresis to the filtered GPS speed accuracy data
|
||||
if (sAccFilterState2 > 1.5f ) {
|
||||
gpsSpdAccPass = false;
|
||||
} else if(sAccFilterState2 < 1.0f) {
|
||||
gpsSpdAccPass = true;
|
||||
}
|
||||
|
||||
// Apply a threshold test with hysteresis to the normalised position and velocity innovations
|
||||
// Require a fail for one second and a pass for 10 seconds to transition
|
||||
if (lastInnovFailTime_ms == 0) {
|
||||
lastInnovFailTime_ms = imuSampleTime_ms;
|
||||
lastInnovPassTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
if (velTestRatio < 1.0f && posTestRatio < 1.0f) {
|
||||
lastInnovPassTime_ms = imuSampleTime_ms;
|
||||
} else if (velTestRatio > 0.7f || posTestRatio > 0.7f) {
|
||||
lastInnovFailTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) {
|
||||
ekfInnovationsPass = false;
|
||||
} else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) {
|
||||
ekfInnovationsPass = true;
|
||||
}
|
||||
|
||||
// both GPS speed accuracy and EKF innovations must pass
|
||||
gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass;
|
||||
}
|
||||
|
||||
// Detect if we are in flight or on ground
|
||||
void NavEKF3_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(hgtMea) > 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 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 && ((rangeDataNew.rng - 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) {
|
||||
// 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 = rangeDataNew.rng;
|
||||
// if the magnetic field states have been set, then continue to update the vertical position
|
||||
// quaternion and yaw innovation snapshots to use as a reference when we start to fly.
|
||||
if (magStateInitComplete) {
|
||||
posDownAtLastMagReset = stateStruct.position.z;
|
||||
quatAtLastMagReset = stateStruct.quat;
|
||||
yawInnovAtLastMagReset = innovYaw;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect
|
||||
bool NavEKF3_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 NavEKF3_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 NavEKF3_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 NavEKF3_core::setTouchdownExpected(bool val)
|
||||
{
|
||||
touchdownExpectedSet_ms = imuSampleTime_ms;
|
||||
expectGndEffectTouchdown = val;
|
||||
}
|
||||
|
||||
// Set to true if the terrain underneath is stable enough to be used as a height reference
|
||||
// in combination with a range finder. Set to false if the terrain underneath the vehicle
|
||||
// cannot be used as a height reference
|
||||
void NavEKF3_core::setTerrainHgtStable(bool val)
|
||||
{
|
||||
terrainHgtStableSet_ms = imuSampleTime_ms;
|
||||
terrainHgtStable = val;
|
||||
}
|
||||
|
||||
// Detect takeoff for optical flow navigation
|
||||
void NavEKF3_core::detectOptFlowTakeoff(void)
|
||||
{
|
||||
if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
|
||||
// we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
|
||||
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) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)));
|
||||
} else if (onGround) {
|
||||
// we are confidently on the ground so set the takeoff detected status to false
|
||||
takeOffDetected = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
1649
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Normal file
1649
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1135
libraries/AP_NavEKF3/AP_NavEKF3_core.h
Normal file
1135
libraries/AP_NavEKF3/AP_NavEKF3_core.h
Normal file
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user