fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 14:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1794 from PX4/ekf-fixes

Browse Source 
EKF Fixes from @Zefz
This commit is contained in:
Lorenz Meier
2015-02-15 22:55:09 +01:00
parent 3f69db2537 c26b4e123f
commit f26a1cb3f1
10 changed files with 1613 additions and 1400 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+26 -15
View File
@@ -162,7 +162,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
namespace
{
GPS *g_dev;
GPS *g_dev = nullptr;
}
@@ -292,7 +292,6 @@ GPS::task_main()
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
@@ -309,10 +308,11 @@ GPS::task_main()
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(_pub_blocked)) {
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
@@ -364,6 +364,7 @@ GPS::task_main()
if (!(_pub_blocked)) {
if (helper_ret & 1) {
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
@@ -478,25 +479,35 @@ GPS::cmd_reset()
void
GPS::print_info()
{
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
//GPS Mode
if(_fake_gps) {
warnx("protocol: SIMULATED");
}
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
else {
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH");
break;
default:
break;
default:
break;
}
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
warnx("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
@@ -520,7 +531,7 @@ GPS::print_info()
namespace gps
{
GPS *g_dev;
GPS *g_dev = nullptr;
void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop();
@@ -686,5 +697,5 @@ gps_main(int argc, char *argv[])
gps::info();
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
}
src/modules/commander/commander.cpp
+16 -22
View File
@@ -1388,29 +1388,23 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_threshold * 2.5f) {
eph_good = false;
} else {
eph_good = true;
}
} else {
if (global_position.eph < eph_threshold) {
eph_good = true;
} else {
eph_good = false;
//update condition_global_position_valid
//Global positions are only published by the estimators if they are valid
if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
//We have had no good fix for POSITION_TIMEOUT amount of time
if(status.condition_global_position_valid) {
set_tune_override(TONE_GPS_WARNING_TUNE);
status_changed = true;
status.condition_global_position_valid = false;
}
}
else if(global_position.timestamp != 0) {
//Got good global position estimate
if(!status.condition_global_position_valid) {
status_changed = true;
status.condition_global_position_valid = true;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
&status_changed);
/* update condition_local_position_valid and condition_local_altitude_valid */
/* hysteresis for EPH */
@@ -2119,7 +2113,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
} else {
if (status_local->condition_local_position_valid) {
if (status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
src/modules/commander/commander_helper.cpp
+5
View File
@@ -115,6 +115,11 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune_override(int tune)
{
ioctl(buzzer, TONE_SET_ALARM, tune);
}
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
src/modules/commander/commander_helper.h
+1
View File
@@ -55,6 +55,7 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
void set_tune_override(int tune);
void set_tune(int tune);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+341
View File
@@ -0,0 +1,341 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file AttitudePositionEstimatorEKF.h
* Implementation of the attitude and position estimator. This is a PX4 wrapper around
* the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp)
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
//Forward declaration
class AttPosEKF;
class AttitudePositionEstimatorEKF
{
public:
/**
* Constructor
*/
AttitudePositionEstimatorEKF();
/* we do not want people ever copying this class */
AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete;
AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete;
/**
* Destructor, also kills the sensors task.
*/
~AttitudePositionEstimatorEKF();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
/**
* Enable logging.
*
* @param enable Set to true to enable logging, false to disable
*/
int enable_logging(bool enable);
/**
* Set debug level.
*
* @param debug Desired debug level - 0 to disable.
*/
int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _estimator_task; /**< task handle for sensor task */
int _sensor_combined_sub;
int _distance_sub; /**< distance measurement */
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
int _landDetectorSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
struct airspeed_s _airspeed; /**< airspeed */
struct baro_report _baro; /**< baro readings */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct gyro_scale _gyro_offsets[3];
struct accel_scale _accel_offsets[3];
struct mag_scale _mag_offsets[3];
struct sensor_combined_s _sensor_combined;
struct map_projection_reference_s _pos_ref;
float _baro_ref; /**< barometer reference altitude */
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_intvl; ///< loop rate counter
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
perf_counter_t _perf_mag; ///<local performance counter for mag updates
perf_counter_t _perf_gps; ///<local performance counter for gps updates
perf_counter_t _perf_baro; ///<local performance counter for baro updates
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
perf_counter_t _perf_reset; ///<local performance counter for filter resets
float _gps_alt_filt;
float _baro_alt_filt;
float _covariancePredictionDt; ///< time lapsed since last covariance prediction
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _last_run;
hrt_abstime _distance_last_valid;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
int _gyro_main; ///< index of the main gyroscope
int _accel_main; ///< index of the main accelerometer
int _mag_main; ///< index of the main magnetometer
bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
bool _newDataGps;
bool _newHgtData;
bool _newAdsData;
bool _newDataMag;
bool _newRangeData;
int _mavlink_fd;
struct {
int32_t vel_delay_ms;
int32_t pos_delay_ms;
int32_t height_delay_ms;
int32_t mag_delay_ms;
int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
} _parameters; /**< local copies of interesting parameters */
struct {
param_t vel_delay_ms;
param_t pos_delay_ms;
param_t height_delay_ms;
param_t mag_delay_ms;
param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
private:
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*
*/
void control_update();
/**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main filter task.
*/
void task_main();
/**
* Check filter sanity state
*
* @return zero if ok, non-zero for a filter error condition.
*/
int check_filter_state();
/**
* @brief
* Publish the euler and quaternions for attitude estimation
**/
void publishAttitude();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
**/
void publishLocalPosition();
/**
* @brief
* Publish global position estimation (WSG84 and AMSL).
* A global position estimate is only valid if we have a good GPS fix
**/
void publishGlobalPosition();
/**
* @brief
* Publish wind estimates for north and east in m/s
**/
void publishWindEstimate();
/**
* @brief
* Runs the sensor fusion step of the filter. The parameters determine which of the sensors
* are fused with each other
**/
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed);
/**
* @brief
* Initialize first time good GPS fix so we can get a reference point to calculate local position from
* Should only be required to call once
**/
void initializeGPS();
/**
* @brief
* Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
* flags to true (e.g newDataGps)
**/
void pollData();
};
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+728 -1031
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+253 -212
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf_att_pos_estimator/estimator_22states.h
+164 -118
View File
@@ -1,9 +1,49 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* Neither the name of the {organization} nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/
/**
* @file estimator_22states.h
*
* Definition of the attitude and position estimator.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include "estimator_utilities.h"
#include <cstddef>
const unsigned int n_states = 22;
const unsigned int data_buffer_size = 50;
constexpr size_t EKF_STATE_ESTIMATES = 22;
constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
class AttPosEKF {
@@ -22,7 +62,7 @@ public:
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
* the InitialiseParameters()
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
@@ -49,40 +89,6 @@ public:
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
minFlowRng = 0.3f; //minimum range between ground and flow sensor
moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
}
struct mag_state_struct {
unsigned obsIndex;
float MagPred[3];
@@ -108,25 +114,25 @@ public:
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float resetStates[n_states];
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix
float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains
float states[EKF_STATE_ESTIMATES]; // state matrix
float resetStates[EKF_STATE_ESTIMATES];
float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
@@ -156,7 +162,6 @@ public:
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
@@ -208,7 +213,6 @@ public:
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useGPS; // boolean true if GPS data is being used
bool useAirspeed; ///< boolean true if airspeed data is being used
@@ -227,7 +231,7 @@ public:
unsigned storeIndex;
// Optical Flow error estimation
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
// Two state EKF used to estimate focal length scale factor and terrain position
float Popt[2][2]; // state covariance matrix
@@ -247,115 +251,157 @@ public:
float minFlowRng; // minimum range over which to fuse optical flow measurements
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
void updateDtGpsFilt(float dt);
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseVelposNED();
void FuseMagnetometer();
void FuseMagnetometer();
void FuseAirspeed();
void FuseAirspeed();
void FuseOptFlow();
void FuseOptFlow();
void OpticalFlowEKF();
/**
* @brief
* Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
* This fiter requires optical flow rates that are not motion compensated
* Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
**/
void OpticalFlowEKF();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*FuseOptFlow
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*FuseOptFlow
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void RecallOmega(float *omegaForFusion, uint64_t msec);
void RecallOmega(float *omegaForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
//static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static inline float sq(float valIn) {return valIn * valIn;}
static float sq(float valIn);
/**
* @brief
* Tell the EKF if the vehicle has landed
**/
void setOnGround(const bool isLanded);
static float maxf(float valIn1, float valIn2);
void CovarianceInit();
static float min(float valIn1, float valIn2);
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
void OnGroundCheck();
float ConstrainFloat(float val, float min, float maxf);
void CovarianceInit();
void ConstrainVariances();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
void ConstrainStates();
float ConstrainFloat(float val, float min, float maxf);
void ForceSymmetry();
void ConstrainVariances();
/**
* @brief
* Check the filter inputs and bound its operational state
*
* This check will reset the filter states if required
* due to a failure of consistency or timeout checks.
* it should be run after the measurement data has been
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound(struct ekf_status_report *last_error);
void ConstrainStates();
/**
* @brief
* Reset the filter position states
*
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition();
void ForceSymmetry();
/**
* @brief
* Reset the velocity state.
*/
void ResetVelocity();
int CheckAndBound(struct ekf_status_report *last_error);
void ZeroVariables();
void ResetPosition();
void GetFilterState(struct ekf_status_report *state);
void ResetVelocity();
void GetLastErrorState(struct ekf_status_report *last_error);
void ZeroVariables();
bool StatesNaN();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN();
void InitializeDynamic(float (&initvelNED)[3], float declination);
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
void updateDtVelPosFilt(float dt);
/**
* @brief
* Initializes algorithm parameters to safe default values
**/
void InitialiseParameters();
bool FilterHealthy();
void updateDtVelPosFilt(float dt);
bool GyroOffsetsDiverged();
bool FilterHealthy();
bool VelNEDDiverged();
bool GyroOffsetsDiverged();
void ResetHeight(void);
bool VelNEDDiverged();
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
/**
* @brief
* Reset the height state.
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight();
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
void ResetStoredStates();
private:
bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
};
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+38
View File
@@ -1,3 +1,41 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* Neither the name of the {organization} nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/
/**
* @file estimator_utilities.cpp
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "estimator_utilities.h"
src/modules/ekf_att_pos_estimator/estimator_utilities.h
+41 -2
View File
@@ -1,8 +1,47 @@
#include <math.h>
#include <stdint.h>
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* Neither the name of the {organization} nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/
/**
* @file estimator_utilities.h
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <math.h>
#include <stdint.h>
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f