diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-02-15 22:55:09 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-02-15 22:55:09 +0100 |
commit | f26a1cb3f1e9fcc1e4ad6efed07b210b4e652564 (patch) | |
tree | 173ff80246f578c21d2526cf76e77d2f7630a039 /src/modules | |
parent | 3f69db25371cf76767668b22ab20660b09433c9f (diff) | |
parent | c26b4e123f9e09dec812ca9039612de200cf7f62 (diff) | |
download | px4-firmware-f26a1cb3f1e9fcc1e4ad6efed07b210b4e652564.tar.gz px4-firmware-f26a1cb3f1e9fcc1e4ad6efed07b210b4e652564.tar.bz2 px4-firmware-f26a1cb3f1e9fcc1e4ad6efed07b210b4e652564.zip |
Merge pull request #1794 from PX4/ekf-fixes
EKF Fixes from @Zefz
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/commander.cpp | 36 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 5 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.h | 1 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 341 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 1713 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 465 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.h | 280 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 38 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_utilities.h | 43 |
9 files changed, 1562 insertions, 1360 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f2e72dd0c..242d8a486 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1388,30 +1388,24 @@ 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; + //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.eph < eph_threshold) { - eph_good = true; - - } else { - eph_good = 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 */ bool local_eph_good; @@ -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 { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 60e8be23e..b5ec6c4e6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -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]; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 4a77fe487..cd3db7324 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -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); diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h new file mode 100644 index 000000000..228ffa853 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -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(); +}; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 62965976d..1c79cb61d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -37,8 +37,12 @@ * * @author Paul Riseborough <p_riseborough@live.com.au> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Johan Jansen <jnsn.johan@gmail.com> */ +#include "AttitudePositionEstimatorEKF.h" +#include "estimator_22states.h" + #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> @@ -51,42 +55,22 @@ #include <time.h> #include <float.h> -#define SENSOR_COMBINED_SUB - -#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> -#ifdef SENSOR_COMBINED_SUB -#include <uORB/topics/sensor_combined.h> -#endif #include <arch/board/board.h> -#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/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 <systemlib/param/param.h> #include <systemlib/err.h> -#include <geo/geo.h> -#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mavlink/mavlink_log.h> #include <platforms/px4_defines.h> -#include "estimator_22states.h" +static uint64_t IMUmsec = 0; +static uint64_t IMUusec = 0; + +//Constants +static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure /** * estimator app start / stop handling function @@ -99,10 +83,6 @@ __EXPORT uint32_t millis(); __EXPORT uint64_t getMicros(); -static uint64_t IMUmsec = 0; -static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds - uint32_t millis() { return IMUmsec; @@ -113,248 +93,25 @@ uint64_t getMicros() return IMUusec; } -class FixedwingEstimator -{ -public: - /** - * Constructor - */ - FixedwingEstimator(); - - /* we do not want people ever copying this class */ - FixedwingEstimator(const FixedwingEstimator& that) = delete; - FixedwingEstimator operator=(const FixedwingEstimator&) = delete; - - /** - * Destructor, also kills the sensors task. - */ - ~FixedwingEstimator(); - - /** - * 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 */ -#ifndef SENSOR_COMBINED_SUB - int _gyro_sub; /**< gyro sensor subscription */ - int _accel_sub; /**< accel sensor subscription */ - int _mag_sub; /**< mag sensor subscription */ -#else - int _sensor_combined_sub; -#endif - 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 */ - - 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 gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; - -#ifdef SENSOR_COMBINED_SUB - struct sensor_combined_s _sensor_combined; -#endif - - 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 - - bool _baro_init; - bool _gps_initialized; - hrt_abstime _gps_start_time; - 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 - - 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; - - /** - * 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(); -}; - namespace estimator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; + /* oddly, ERROR is not defined for c++ */ + #ifdef ERROR + # undef ERROR + #endif + static const int ERROR = -1; -FixedwingEstimator *g_estimator = nullptr; + AttitudePositionEstimatorEKF *g_estimator = nullptr; } -FixedwingEstimator::FixedwingEstimator() : - +AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_should_exit(false), _task_running(false), _estimator_task(-1), -/* subscriptions */ -#ifndef SENSOR_COMBINED_SUB - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), -#else + /* subscriptions */ _sensor_combined_sub(-1), -#endif _distance_sub(-1), _airspeed_sub(-1), _baro_sub(-1), @@ -364,8 +121,9 @@ FixedwingEstimator::FixedwingEstimator() : _manual_control_sub(-1), _mission_sub(-1), _home_sub(-1), + _landDetectorSub(-1), -/* publications */ + /* publications */ _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), @@ -384,21 +142,20 @@ FixedwingEstimator::FixedwingEstimator() : _gps({}), _wind({}), _distance{}, + _landDetector{}, _gyro_offsets({}), _accel_offsets({}), _mag_offsets({}), - #ifdef SENSOR_COMBINED_SUB _sensor_combined{}, - #endif _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), @@ -408,10 +165,14 @@ FixedwingEstimator::FixedwingEstimator() : _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), -/* states */ + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), _baro_init(false), _gps_initialized(false), - _gps_start_time(0), _filter_start_time(0), _last_sensor_timestamp(0), _last_run(0), @@ -424,12 +185,18 @@ FixedwingEstimator::FixedwingEstimator() : _mag_main(0), _ekf_logging(true), _debug(0), + + _newDataGps(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + _mavlink_fd(-1), _parameters{}, _parameter_handles{}, _ekf(nullptr) { - _last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); @@ -455,7 +222,6 @@ FixedwingEstimator::FixedwingEstimator() : parameters_update(); /* get offsets */ - int fd, res; for (unsigned s = 0; s < 3; s++) { @@ -498,7 +264,7 @@ FixedwingEstimator::FixedwingEstimator() : } } -FixedwingEstimator::~FixedwingEstimator() +AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() { if (_estimator_task != -1) { @@ -525,16 +291,14 @@ FixedwingEstimator::~FixedwingEstimator() estimator::g_estimator = nullptr; } -int -FixedwingEstimator::enable_logging(bool logging) +int AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; return 0; } -int -FixedwingEstimator::parameters_update() +int AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -578,8 +342,7 @@ FixedwingEstimator::parameters_update() return OK; } -void -FixedwingEstimator::vehicle_status_poll() +void AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -592,8 +355,7 @@ FixedwingEstimator::vehicle_status_poll() } } -int -FixedwingEstimator::check_filter_state() +int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -687,15 +449,15 @@ FixedwingEstimator::check_filter_state() } // Copy all states or at least all that we can fit - unsigned ekf_n_states = ekf_report.n_states; - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + size_t ekf_n_states = ekf_report.n_states; + size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } - for (unsigned i = 0; i < rep.n_states; i++) { + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; } @@ -709,19 +471,17 @@ FixedwingEstimator::check_filter_state() return check; } -void -FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } -void -FixedwingEstimator::task_main() +void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); - float dt = 0.0f; // time lapsed since last covariance prediction + _filter_start_time = hrt_absolute_time(); if (!_ekf) { @@ -738,63 +498,32 @@ FixedwingEstimator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); + _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); -#ifndef SENSOR_COMBINED_SUB - - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - /* rate limit gyro updates to 50 Hz */ - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 4); -#else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); -#endif /* sets also parameters in the EKF object */ parameters_update(); - Vector3f lastAngRate; - Vector3f lastAccel; - /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; -#ifndef SENSOR_COMBINED_SUB - fds[1].fd = _gyro_sub; - fds[1].events = POLLIN; -#else + fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; -#endif - - bool newDataGps = false; - bool newHgtData = false; - bool newAdsData = false; - bool newDataMag = false; - bool newRangeData = false; - - float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) - uint64_t last_gps = 0; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; - // init lowpass filters for baro and gps altitude - float _gps_alt_filt = 0, _baro_alt_filt = 0; - float rc = 10.0f; // RC time constant of 1st order LPF in seconds - hrt_abstime baro_last = 0; - _task_running = true; while (!_task_should_exit) { @@ -832,9 +561,6 @@ FixedwingEstimator::task_main() bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); - bool accel_updated; - bool mag_updated; - perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ @@ -842,14 +568,8 @@ FixedwingEstimator::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); -#else /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); -#endif /* set sensors to de-initialized state */ _gyro_valid = false; @@ -872,848 +592,825 @@ FixedwingEstimator::task_main() /** * PART ONE: COLLECT ALL DATA **/ + pollData(); - /* load local copies */ -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + /* + * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY + */ + if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { + continue; + } + /** + * PART TWO: EXECUTE THE FILTER + * + * We run the filter only once all data has been fetched + **/ - orb_check(_accel_sub, &accel_updated); + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } - _last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3; - IMUusec = _gyro.timestamp; + /* Initialize the filter first */ + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + } + else if (!_ekf->statesInitialised) { + // North, East Down position (m) + float initVelNED[3] = {0.0f, 0.0f, 0.0f}; - float deltaT = (_gyro.timestamp - _last_run) / 1e6f; - _last_run = _gyro.timestamp; + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } + _local_pos.ref_alt = _baro_ref; + _baro_ref_offset = 0.0f; + _baro_gps_offset = 0.0f; + _baro_alt_filt = _baro.altitude; + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; + } + else if (_ekf->statesInitialised) { - if (isfinite(_gyro.x) && - isfinite(_gyro.y) && - isfinite(_gyro.z)) { - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + // Check if on ground - status is used by covariance prediction + _ekf->setOnGround(_landDetector.landed); - if (!_gyro_valid) { - lastAngRate = _ekf->angRate; - } + // We're apparently initialized in this case now + // check (and reset the filter as needed) + int check = check_filter_state(); + if (check) { + // Let the system re-initialize itself + continue; + } - _gyro_valid = true; - } + //Run EKF data fusion steps + updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - if (accel_updated) { - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + //Publish attitude estimations + publishAttitude(); - if (!_accel_valid) { - lastAccel = _ekf->accel; - } + //Publish Local Position estimations + publishLocalPosition(); + + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gpsIsGood) + { + publishGlobalPosition(); + } - _accel_valid = true; + //Publish wind estimates + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + publishWindEstimate(); + } + } } - _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - _ekf->lastAngRate = angRate; - _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - _ekf->lastAccel = accel; + } + perf_end(_loop_perf); + } -#else - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + _task_running = false; - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; + warnx("exiting.\n"); - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; - } else { - accel_updated = false; - } + _estimator_task = -1; + _exit(0); +} - last_accel = _sensor_combined.accelerometer_timestamp; +void AttitudePositionEstimatorEKF::initializeGPS() +{ + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; - IMUusec = _sensor_combined.timestamp; + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = _baro.altitude; - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); - /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; - _last_run = _sensor_combined.timestamp; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); - int last_gyro_main = _gyro_main; + float initVelNED[3]; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; - if (isfinite(_sensor_combined.gyro_rad_s[0]) && - isfinite(_sensor_combined.gyro_rad_s[1]) && - isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - _gyro_main = 0; - _gyro_valid = true; + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = _gps.timestamp_position; - } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && - isfinite(_sensor_combined.gyro1_rad_s[1]) && - isfinite(_sensor_combined.gyro1_rad_s[2])) { + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; + #if 0 + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); + #endif - } else { - _gyro_valid = false; - } + _gps_initialized = true; +} - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); - } +void AttitudePositionEstimatorEKF::publishAttitude() +{ + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; - } else { - perf_count(_perf_gyro); - } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(_att.R, i, j) = R(i, j); + } + } - if (accel_updated) { - - int last_accel_main = _accel_main; - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - _accel_main = 0; - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } + _att.timestamp = _last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = _last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - if (!_accel_valid) { - lastAccel = _ekf->accel; - } + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } +} - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } +void AttitudePositionEstimatorEKF::publishLocalPosition() +{ + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; - _accel_valid = true; - } + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; - if (last_mag != _sensor_combined.magnetometer_timestamp) { - mag_updated = true; - newDataMag = true; + _local_pos.xy_valid = _gps_initialized && _gpsIsGood; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized && _gpsIsGood; + _local_pos.v_z_valid = true; + _local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here - } else { - newDataMag = false; - } + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; - last_mag = _sensor_combined.magnetometer_timestamp; + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); -#endif + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } +} - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); +void AttitudePositionEstimatorEKF::publishGlobalPosition() +{ + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_utc_usec = _gps.time_utc_usec; + } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; - newAdsData = true; + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } - } else { - newAdsData = false; - } + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - bool gps_updated; - orb_check(_gps_sub, &gps_updated); + _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; - if (gps_updated) { + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); - perf_count(_perf_gps); + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } +} - if (_gps.fix_type < 3) { - newDataGps = false; +void AttitudePositionEstimatorEKF::publishWindEstimate() +{ + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; - } else { + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; - /* store time of valid GPS measurement */ - _gps_start_time = hrt_absolute_time(); + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - /* check if we had a GPS outage for a long time */ - float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } - const float pos_reset_threshold = 5.0f; // seconds +} - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { - _ekf->ResetPosition(); - _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); - } - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); +void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, + const bool fuseBaro, const bool fuseAirSpeed) +{ + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + _covariancePredictionDt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(_covariancePredictionDt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + _covariancePredictionDt = 0.0f; + } - /* fuse GPS updates */ + // Fuse GPS Measurements + if (fuseGPS && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED - //_gps.timestamp / 1e3; - _ekf->GPSstatus = _gps.fix_type; - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; - // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + // run the fusion step + _ekf->FuseVelposNED(); - // update LPF - _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); + } + else if (!_gps_initialized) { - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // force static mode + _ekf->staticMode = true; - // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); - // } else { - // _ekf->vneSigma = _parameters.velne_noise; - // } + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; - // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); - // } else { - // _ekf->posNeSigma = _parameters.posne_noise; - // } + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; - newDataGps = true; - last_gps = _gps.timestamp_position; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - } + // run the fusion step + _ekf->FuseVelposNED(); - } + } + else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + if (fuseBaro) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; - if (baro_updated) { + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + // run the fusion step + _ekf->FuseVelposNED(); - float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; - baro_last = _baro.timestamp; + } + else { + _ekf->fuseHgtData = false; + } - _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); + // Fuse Magnetometer Measurements + if (fuseMag) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } + } + else { + _ekf->fuseMagData = false; + } - perf_count(_perf_baro); + // Fuse Airspeed Measurements + if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); - newHgtData = true; - } else { - newHgtData = false; - } + } + else { + _ekf->fuseVtasData = false; + } -#ifndef SENSOR_COMBINED_SUB - orb_check(_mag_sub, &mag_updated); -#endif + // Fuse Rangefinder Measurements + if (fuseRangeSensor) { + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } + } +} - if (mag_updated) { +int AttitudePositionEstimatorEKF::start() +{ + ASSERT(_estimator_task == -1); - _mag_valid = true; + /* start the task */ + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 7500, + (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + nullptr); - perf_count(_perf_mag); + if (_estimator_task < 0) { + warn("task start failed"); + return -errno; + } -#ifndef SENSOR_COMBINED_SUB - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + return OK; +} - // XXX we compensate the offsets upfront - should be close to zero. - // 0.001f - _ekf->magData.x = _mag.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset +void AttitudePositionEstimatorEKF::print_status() +{ + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); - _ekf->magData.y = _mag.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); - _ekf->magData.z = _mag.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13: Accelerometer offset + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) -#else - int last_mag_main = _mag_main; + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - // XXX we compensate the offsets upfront - should be close to zero. + if (EKF_STATE_ESTIMATES == 23) { + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); - /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { - _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + } else { + printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + } + printf("states: %s %s %s %s %s %s %s %s %s %s\n", + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); +} - _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset +void AttitudePositionEstimatorEKF::pollData() +{ + static Vector3f lastAngRate; + static Vector3f lastAccel; + bool accel_updated = false; - _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - } else { - _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + //Update Gyro and Accelerometer + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + static hrt_abstime last_accel = 0; + static hrt_abstime last_mag = 0; - _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } + if (last_accel != _sensor_combined.accelerometer_timestamp) { + accel_updated = true; + } else { + accel_updated = false; + } - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); - } -#endif + last_accel = _sensor_combined.accelerometer_timestamp; - newDataMag = true; - } else { - newDataMag = false; - } + // Copy gyro and accel + _last_sensor_timestamp = _sensor_combined.timestamp; + IMUmsec = _sensor_combined.timestamp / 1e3; + IMUusec = _sensor_combined.timestamp; - orb_check(_distance_sub, &newRangeData); + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; - if (newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; - } else { - newRangeData = false; - } - } + _last_run = _sensor_combined.timestamp; - /* - * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY - */ - if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { - continue; - } + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; - /** - * PART TWO: EXECUTE THE FILTER - * - * We run the filter only once all data has been fetched - **/ + int last_gyro_main = _gyro_main; - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2]) && + (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { - float initVelNED[3]; + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_main = 0; + _gyro_valid = true; - // maintain filtered baro and gps altitudes to calculate weather offset - // baro sample rate is ~70Hz and measurement bandwidth is high - // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds - // maintain heavily filtered values for both baro and gps altitude - // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; -// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { -// _last_debug_print = hrt_absolute_time(); -// perf_print_counter(_perf_baro); -// perf_reset(_perf_baro); -// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", -// (double)_baro_gps_offset, -// (double)_baro_alt_filt, -// (double)_gps_alt_filt, -// (double)_global_pos.alt, -// (double)_local_pos.z); -// } + } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && + isfinite(_sensor_combined.gyro1_rad_s[1]) && + isfinite(_sensor_combined.gyro1_rad_s[2])) { - /* Initialize the filter first */ - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { + _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; + _gyro_main = 1; + _gyro_valid = true; - // GPS is in scaled integers, convert - double lat = _gps.lat / 1.0e7; - double lon = _gps.lon / 1.0e7; - float gps_alt = _gps.alt / 1e3f; + } else { + _gyro_valid = false; + } - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; + if (last_gyro_main != _gyro_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); + } - // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + if (!_gyro_valid) { + /* keep last value if he hit an out of band value */ + lastAngRate = _ekf->angRate; + } else { + perf_count(_perf_gyro); + } - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; + if (accel_updated) { - _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + int last_accel_main = _accel_main; - // Set up position variables correctly - _ekf->GPSstatus = _gps.fix_type; + /* fail over to the 2nd accel if we know the first is down */ + if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_main = 0; + } else { + _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; + _accel_main = 1; + } - _ekf->gpsLat = math::radians(lat); - _ekf->gpsLon = math::radians(lon) - M_PI; - _ekf->gpsHgt = gps_alt; + if (!_accel_valid) { + lastAccel = _ekf->accel; + } - // Look up mag declination based on current position - float declination = math::radians(get_mag_declination(lat, lon)); + if (last_accel_main != _accel_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); + } - _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + _accel_valid = true; + } - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + if (last_mag != _sensor_combined.magnetometer_timestamp) { + _newDataMag = true; - #if 0 - warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); - #endif + } else { + _newDataMag = false; + } - _gps_initialized = true; + last_mag = _sensor_combined.magnetometer_timestamp; - } else if (!_ekf->statesInitialised) { + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); - initVelNED[0] = 0.0f; - initVelNED[1] = 0.0f; - initVelNED[2] = 0.0f; - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + //Update Land Detector + bool newLandData; + orb_check(_landDetectorSub, &newLandData); + if(newLandData) { + orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); + } - _local_pos.ref_alt = _baro_ref; - _baro_ref_offset = 0.0f; - _baro_gps_offset = 0.0f; + //Update AirSpeed + orb_check(_airspeed_sub, &_newAdsData); + if (_newAdsData) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + } - } else if (_ekf->statesInitialised) { - // We're apparently initialized in this case now - // check (and reset the filter as needed) - int check = check_filter_state(); + orb_check(_gps_sub, &_newDataGps); + if (_newDataGps) { - if (check) { - // Let the system re-initialize itself - continue; - } + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + perf_count(_perf_gps); - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); + //We are more strict for our first fix + float requiredAccuracy = _parameters.pos_stddev_threshold; + if(_gpsIsGood) { + requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; + } - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->dtIMU; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } + //Check if the GPS fix is good enough for us to use + if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + _gpsIsGood = true; + } + else { + _gpsIsGood = false; + } - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - - float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; - - // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { - - _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; - _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; - _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; - } - - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (!_gps_initialized) { - - // force static mode - _ekf->staticMode = true; - - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - - _ekf->posNE[0] = 0.0f; - _ekf->posNE[1] = 0.0f; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - } + if (_gpsIsGood) { - if (newHgtData) { - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); - _ekf->fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else { - _ekf->fuseHgtData = false; - } + //Calculate time since last good GPS fix + const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; - // Fuse Magnetometer Measurements - if (newDataMag) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); - _ekf->magstate.obsIndex = 0; - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); - _ekf->FuseMagnetometer(); + /* fuse GPS updates */ - } else { - _ekf->fuseMagData = false; - } + //_gps.timestamp / 1e3; + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->VtasMeas > 7.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - } else { - _ekf->fuseVtasData = false; - } + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; - if (newRangeData) { + //check if we had a GPS outage for a long time + if(_gps_initialized) { - if (_ekf->Tnb.z.z > 0.9f) { - // _ekf->rngMea is set in sensor readout already - _ekf->fuseRngData = true; - _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->OpticalFlowEKF(); - _ekf->fuseRngData = false; - } - } + //Convert from global frame to local frame + float posNED[3] = {0.0f, 0.0f, 0.0f}; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + if (dtGoodGPS > POS_RESET_THRESHOLD) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + } + } - // Output results - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - PX4_R(_att.R, i, j) = R(i, j); - - _att.timestamp = _last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = _last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + // update LPF + _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - if (_gps_initialized) { - _local_pos.timestamp = _last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; - - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; - - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; - - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; - - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); - - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } - - _global_pos.timestamp = _local_pos.timestamp; - - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_utc_usec = _gps.time_utc_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } - - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } - - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; - - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - - _global_pos.yaw = _local_pos.yaw; - - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - - _global_pos.timestamp = _local_pos.timestamp; - - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); - } - - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _wind.covariance_north = _ekf->P[14][14]; - _wind.covariance_east = _ekf->P[15][15]; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - } + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); - } + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } - } + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } - } + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + _previousGPSTimestamp = _gps.timestamp_position; } + else { + //Too poor GPS fix to use + _newDataGps = false; + } + } - perf_end(_loop_perf); + // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, + // then something is very wrong with the GPS (possibly a hardware failure or comlink error) + else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { + _gpsIsGood = false; } - _task_running = false; + //Update barometer + orb_check(_baro_sub, &_newHgtData); - warnx("exiting.\n"); + if (_newHgtData) { + static hrt_abstime baro_last = 0; - _estimator_task = -1; - _exit(0); -} + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); -int -FixedwingEstimator::start() -{ - ASSERT(_estimator_task == -1); + // init lowpass filters for baro and gps altitude + float baro_elapsed; + if(baro_last == 0) { + baro_elapsed = 0.0f; + } + else { + baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + } + baro_last = _baro.timestamp; - /* start the task */ - _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 7500, - (main_t)&FixedwingEstimator::task_main_trampoline, - nullptr); + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); - if (_estimator_task < 0) { - warn("task start failed"); - return -errno; + _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + perf_count(_perf_baro); + + _newHgtData = true; } - return OK; -} + //Update Magnetometer + if (_newDataMag) { -void -FixedwingEstimator::print_status() -{ - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); + _mag_valid = true; - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + perf_count(_perf_mag); - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Accelerometer offset - // 14-15: Wind Vector - m/sec (North,East) - // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) - // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + int last_mag_main = _mag_main; - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + // XXX we compensate the offsets upfront - should be close to zero. - if (n_states == 23) { - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + /* fail over to the 2nd mag if we know the first is down */ + if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - } else { - printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 0; + } else { + _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + _mag_main = 1; + } + + if (last_mag_main != _mag_main) { + mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); + } + } + + //Update range data + orb_check(_distance_sub, &_newRangeData); + if (_newRangeData) { + orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + + if (_distance.valid) { + _ekf->rngMea = _distance.distance; + _distance_last_valid = _distance.timestamp; + } else { + _newRangeData = false; + } } - printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int FixedwingEstimator::trip_nan() { +int AttitudePositionEstimatorEKF::trip_nan() { int ret = 0; @@ -1772,7 +1469,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator != nullptr) errx(1, "already running"); - estimator::g_estimator = new FixedwingEstimator; + estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) errx(1, "alloc failed"); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 15d018ab6..d8e3116b9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,14 +1,54 @@ +/**************************************************************************** +* 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.cpp + * + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough <p_riseborough@live.com.au> + * @author Lorenz Meier <lorenz@px4.io> + */ + #include "estimator_22states.h" #include <string.h> #include <stdio.h> #include <stdarg.h> #include <math.h> +#include <algorithm> #ifndef M_PI_F -#define M_PI_F ((float)M_PI) +#define M_PI_F static_cast<float>(M_PI) #endif -#define EKF_COVARIANCE_DIVERGED 1.0e8f +constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), @@ -72,7 +112,6 @@ AttPosEKF::AttPosEKF() : innovVelPos{}, varInnovVelPos{}, velNED{}, - accelGPSNED{}, posNE{}, hgtMea(0.0f), baroHgtOffset(0.0f), @@ -115,7 +154,6 @@ AttPosEKF::AttPosEKF() : inhibitGndState(true), inhibitScaleState(true), - onGround(true), staticMode(true), useGPS(false), useAirspeed(true), @@ -145,7 +183,9 @@ AttPosEKF::AttPosEKF() : auxFlowInnovGate(0.0f), rngInnovGate(0.0f), minFlowRng(0.0f), - moCompR_LOS(0.0f) + moCompR_LOS(0.0f), + + _onGround(true) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -158,6 +198,40 @@ AttPosEKF::~AttPosEKF() { } +void AttPosEKF::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 +} + + void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; @@ -177,7 +251,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float deltaQuat[4]; const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS); -// Remove sensor bias errors + // Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; @@ -192,14 +266,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; -// Save current measurements + // Save current measurements Vector3f prevDelAng = correctedDelAng; -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded + // Apply corrections for earths rotation rate and coning errors + // * and + operators have been overloaded correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion + // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -221,14 +295,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED() deltaQuat[3] = correctedDelAng.z*rotScaler; } -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion + // Update the quaternions by rotating from the previous attitude through + // the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; -// Normalise the quaternions and update the quaternion states + // Normalise the quaternions and update the quaternion states quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { @@ -239,7 +313,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() states[3] = quatMagInv*qUpdated[3]; } -// Calculate the body to nav cosine matrix + // Calculate the body to nav cosine matrix q00 = sq(states[0]); q11 = sq(states[1]); q22 = sq(states[2]); @@ -263,29 +337,29 @@ void AttPosEKF::UpdateStrapdownEquationsNED() Tnb = Tbn.transpose(); -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded + // transform body delta velocities to delta velocities in the nav frame + // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) + // calculate the magnitude of the nav acceleration (required for GPS + // variance estimation) accNavMag = delVelNav.length()/dtIMU; -// If calculating position save previous velocity + // If calculating position save previous velocity float lastVelocity[3]; lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; -// Sum delta velocities to get velocity + // Sum delta velocities to get velocity states[4] = states[4] + delVelNav.x; states[5] = states[5] + delVelNav.y; states[6] = states[6] + delVelNav.z; -// If calculating postions, do a trapezoidal integration for position + // If calculating postions, do a trapezoidal integration for position states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; @@ -322,12 +396,12 @@ void AttPosEKF::CovariancePrediction(float dt) float dvz_b; // arrays - float processNoise[n_states]; + float processNoise[EKF_STATE_ESTIMATES]; float SF[15]; float SG[8]; float SQ[11]; float SPP[8] = {0}; - float nextP[n_states][n_states]; + float nextP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; @@ -343,13 +417,13 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = 0; } // square all sigmas - for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -370,7 +444,7 @@ void AttPosEKF::CovariancePrediction(float dt) daxCov = sq(dt*gyroProcessNoise); dayCov = sq(dt*gyroProcessNoise); dazCov = sq(dt*gyroProcessNoise); - if (onGround) dazCov = dazCov * sq(yawVarScale); + if (_onGround) dazCov = dazCov * sq(yawVarScale); accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); dvxCov = sq(dt*accelProcessNoise); dvyCov = sq(dt*accelProcessNoise); @@ -908,7 +982,7 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - for (unsigned i = 0; i < n_states; i++) + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -921,7 +995,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (unsigned j = 0; j < n_states; j++) + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -930,12 +1004,12 @@ void AttPosEKF::CovariancePrediction(float dt) } // Copy covariance - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { P[i][i] = nextP[i][i]; } // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -965,24 +1039,24 @@ void AttPosEKF::updateDtVelPosFilt(float dt) void AttPosEKF::FuseVelposNED() { -// declare variables used by fault isolation logic + // declare variables used by fault isolation logic uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; -// declare variables used to check measurement errors + // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; float posInnov[2] = {0.0f,0.0f}; float hgtInnov = 0.0f; -// declare variables used to control access to arrays + // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; uint8_t indexLimit = 21; -// declare variables used by state and covariance update calculations + // declare variables used by state and covariance update calculations float velErr; float posErr; float R_OBS[6]; @@ -990,12 +1064,12 @@ void AttPosEKF::FuseVelposNED() float SK; float quatMag; -// 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 + // 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) { uint64_t tNow = getMicros(); @@ -1015,8 +1089,8 @@ void AttPosEKF::FuseVelposNED() else horizRetryTime = gpsRetryTimeNoTAS; // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + for (uint8_t i=0; i <=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i <=4; i++) observation[i] = posNE[i-3]; observation[5] = -(hgtMea); // Estimate the GPS Velocity, GPS horiz position and height measurement variances. @@ -1048,10 +1122,9 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { - // XXX check current_ekf_state.velHealth = true; ResetVelocity(); - ResetStoredStates(); + // do not fuse bad data fuseVelData = false; } @@ -1060,6 +1133,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.velHealth = false; } } + if (fusePosData) { // test horizontal position measurements @@ -1078,9 +1152,6 @@ void AttPosEKF::FuseVelposNED() if (current_ekf_state.posTimeout) { ResetPosition(); - // XXX cross-check the state reset - ResetStoredStates(); - // do not fuse position data on this time // step fusePosData = false; @@ -1091,6 +1162,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.posHealth = false; } } + // test height measurements if (fuseHgtData) { @@ -1108,7 +1180,6 @@ void AttPosEKF::FuseVelposNED() // the height data, but reset height and stored states if (current_ekf_state.hgtTimeout) { ResetHeight(); - ResetStoredStates(); fuseHgtData = false; } } @@ -1179,7 +1250,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i < n_states; i++) + for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } @@ -1190,7 +1261,7 @@ void AttPosEKF::FuseVelposNED() { states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) // divide by 0 protection { for (uint8_t i = 0; i<=3; i++) @@ -1246,17 +1317,17 @@ void AttPosEKF::FuseMagnetometer() float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - float H_MAG[n_states]; - for (uint8_t i = 0; i < n_states; i++) { + float H_MAG[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { H_MAG[i] = 0.0f; } -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion + // Perform sequential fusion of Magnetometer measurements. + // This assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion if (useCompass && fuseMagData && (obsIndex < 3)) { // Calculate observation jacobians and Kalman gains @@ -1303,7 +1374,7 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; @@ -1360,7 +1431,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1370,7 +1441,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[2]; H_MAG[1] = SH_MAG[1]; H_MAG[2] = SH_MAG[0]; @@ -1439,7 +1510,7 @@ void AttPosEKF::FuseMagnetometer() else if (obsIndex == 2) // we are now fusing the Z measurement { // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[1]; H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; @@ -1512,12 +1583,12 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j= 0; j < n_states; j++) + for (uint8_t j= 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1529,40 +1600,40 @@ void AttPosEKF::FuseMagnetometer() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; - if (!onGround) + if (!_onGround) { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j < n_states; j++) + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } - if (!onGround) + if (!_onGround) { - for (uint8_t k = 16; k < n_states; k++) + for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1570,9 +1641,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1610,12 +1681,12 @@ void AttPosEKF::FuseAirspeed() if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - float H_TAS[n_states]; - for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; + float H_TAS[EKF_STATE_ESTIMATES]; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; H_TAS[5] = SH_TAS[1]; H_TAS[6] = vd*SH_TAS[0]; @@ -1664,7 +1735,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i < n_states; i++) { + for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) { Kfusion[i] = 0; } } @@ -1676,12 +1747,12 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j < n_states; j++) + for (uint8_t j=0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j <= 3; j++) @@ -1693,7 +1764,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1705,11 +1776,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1722,9 +1793,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1736,13 +1807,13 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (row=first; row<=last; row++) { - for (col=0; col<n_states; col++) + for (col=0; col<EKF_STATE_ESTIMATES; col++) { covMat[row][col] = 0.0; } @@ -1765,13 +1836,13 @@ void AttPosEKF::FuseOptFlow() static float losPred[2]; // Transformation matrix from nav to body axes - float H_LOS[2][n_states]; - float K_LOS[2][n_states]; + float H_LOS[2][EKF_STATE_ESTIMATES]; + float K_LOS[2][EKF_STATE_ESTIMATES]; Vector3f velNED_local; Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only with valid tilt and height - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; bool validTilt = Tnb.z.z > 0.71f; if (validTilt) @@ -1836,7 +1907,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); // calculate observation jacobians for X LOS rate - for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[0][i] = 0; H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); @@ -1877,7 +1948,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); } else { - for (uint8_t i = 16; i < n_states; i++) { + for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { K_LOS[0][i] = 0.0f; } } @@ -1898,7 +1969,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; // Calculate observation jacobians for Y LOS rate - for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0; + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[1][i] = 0; H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; @@ -1939,7 +2010,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); } else { - for (uint8_t i = 16; i < n_states; i++) { + for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { K_LOS[1][i] = 0.0f; } } @@ -1955,12 +2026,12 @@ void AttPosEKF::FuseOptFlow() // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { // correct the state vector - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; } // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1972,7 +2043,7 @@ void AttPosEKF::FuseOptFlow() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the // number of operations - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j <= 6; j++) { @@ -1983,14 +2054,14 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j < n_states; j++) + for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 6; k++) @@ -2000,9 +2071,9 @@ void AttPosEKF::FuseOptFlow() KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; } } - for (uint8_t i = 0; i < n_states; i++) + for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - for (uint8_t j = 0; j < n_states; j++) + for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2014,11 +2085,6 @@ void AttPosEKF::FuseOptFlow() } } -/* -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 AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption @@ -2036,7 +2102,7 @@ void AttPosEKF::OpticalFlowEKF() } else { return; } - distanceTravelledSq = min(distanceTravelledSq, 100.0f); + distanceTravelledSq = std::min(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } @@ -2076,7 +2142,7 @@ void AttPosEKF::OpticalFlowEKF() varInnovRng = 1.0f/SK_RNG[1]; // constrain terrain height to be below the vehicle - flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; @@ -2096,7 +2162,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix float nextPopt[2][2]; @@ -2105,8 +2171,8 @@ void AttPosEKF::OpticalFlowEKF() nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = maxf(nextPopt[0][0],0.0f); - Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][0] = std::max(nextPopt[0][0],0.0f); + Popt[1][1] = std::max(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2145,7 +2211,7 @@ void AttPosEKF::OpticalFlowEKF() vel.z = statesAtFlowTime[6]; // constrain terrain height to be below the vehicle - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; @@ -2213,7 +2279,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); // correct the covariance matrix for (uint8_t i = 0; i < 2 ; i++) { @@ -2229,8 +2295,8 @@ void AttPosEKF::OpticalFlowEKF() } // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = maxf(nextPopt[0][0],0.0f); - Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][0] = std::max(nextPopt[0][0],0.0f); + Popt[1][1] = std::max(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2239,53 +2305,30 @@ void AttPosEKF::OpticalFlowEKF() } -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; for (col=first; col<=last; col++) { - for (row=0; row < n_states; row++) + for (row=0; row < EKF_STATE_ESTIMATES; row++) { covMat[row][col] = 0.0; } } } -float AttPosEKF::sq(float valIn) -{ - return valIn*valIn; -} - -float AttPosEKF::maxf(float valIn1, float valIn2) -{ - if (valIn1 >= valIn2) { - return valIn1; - } else { - return valIn2; - } -} - -float AttPosEKF::min(float valIn1, float valIn2) -{ - if (valIn1 <= valIn2) { - return valIn1; - } else { - return valIn2; - } -} - // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (unsigned i=0; i<n_states; i++) + for (size_t i=0; i<EKF_STATE_ESTIMATES; i++) storedStates[i][storeIndex] = states[i]; storedOmega[0][storeIndex] = angRate.x; storedOmega[1][storeIndex] = angRate.y; storedOmega[2][storeIndex] = angRate.z; statetimeStamp[storeIndex] = timestamp_ms; storeIndex++; - if (storeIndex == data_buffer_size) + if (storeIndex == EKF_DATA_BUFFER_SIZE) storeIndex = 0; } @@ -2311,8 +2354,8 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) int ret = 0; int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) + size_t bestStoreIndex = 0; + for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++) { // Work around a GCC compiler bug - we know 64bit support on ARM is // sketchy in GCC. @@ -2332,7 +2375,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) } if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error { - for (unsigned i=0; i < n_states; i++) { + for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) { if (isfinite(storedStates[i][bestStoreIndex])) { statesForFusion[i] = storedStates[i][bestStoreIndex]; } else if (isfinite(states[i])) { @@ -2346,7 +2389,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) } else // otherwise output current state { - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { @@ -2361,25 +2404,25 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec) { // work back in time and calculate average angular rate over the time interval - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] = 0.0f; } uint8_t sumIndex = 0; int64_t timeDelta; - for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) + for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++) { // calculate the average of all samples younger than msec timeDelta = statetimeStamp[storeIndexLocal] - msec; if (timeDelta > 0) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] += storedOmega[i][storeIndexLocal]; } sumIndex += 1; } } if (sumIndex >= 1) { - for (unsigned i=0; i < 3; i++) { + for (size_t i=0; i < 3; i++) { omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); } } else { @@ -2389,6 +2432,7 @@ void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec) } } +#if 0 void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2413,6 +2457,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.x.z = 2*(q13 - q02); Tnb.y.z = 2*(q23 + q01); } +#endif void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { @@ -2481,37 +2526,41 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d hgt = hgtRef - posNED[2]; } -void AttPosEKF::OnGroundCheck() +void AttPosEKF::setOnGround(const bool isLanded) { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + _onGround = isLanded; if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } // don't update wind states if there is no airspeed measurement - if (onGround || !useAirspeed) { + if (_onGround || !useAirspeed) { inhibitWindStates = true; } else { inhibitWindStates =false; } + // don't update magnetic field states if on ground or not using compass - if (onGround || !useCompass) { + if (_onGround || !useCompass) { inhibitMagStates = true; } else { inhibitMagStates = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS - if ((onGround || !useGPS) && !useRangeFinder) { + if ((_onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable - if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // Don't update focal length offset state if there is no range finder or optical flow sensor // we need both sensors to do this estimation if (!useRangeFinder || !useOpticalFlow) { @@ -2596,22 +2645,22 @@ void AttPosEKF::ConstrainVariances() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion variances - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Constrain velocity variances - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i <= 9; i++) { + for (size_t i = 7; i <= 9; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); } // Constrain delta angle bias variances - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } @@ -2619,17 +2668,17 @@ void AttPosEKF::ConstrainVariances() P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); // Wind velocity variances - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Earth magnetic field variances - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Body magnetic field variances - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } @@ -2652,17 +2701,17 @@ void AttPosEKF::ConstrainStates() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain quaternion - for (unsigned i = 0; i <= 3; i++) { + for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i <= 6; i++) { + for (size_t i = 4; i <= 6; i++) { states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); } // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i <= 8; i++) { + for (size_t i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -2671,7 +2720,7 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i <= 12; i++) { + for (size_t i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } @@ -2679,18 +2728,18 @@ void AttPosEKF::ConstrainStates() states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 14; i <= 15; i++) { + for (size_t i = 14; i <= 15; i++) { states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); } // Earth magnetic field limits (in Gauss) - for (unsigned i = 16; i <= 18; i++) { + for (size_t i = 16; i <= 18; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Body magnetic field variances (in Gauss). // the max offset should be in this range. - for (unsigned i = 19; i <= 21; i++) { + for (size_t i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } @@ -2704,7 +2753,7 @@ void AttPosEKF::ForceSymmetry() // Force symmetry on the covariance matrix to prevent ill-conditioning // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) + for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++) { for (uint8_t j = 0; j < i; j++) { @@ -2792,12 +2841,6 @@ bool AttPosEKF::FilterHealthy() return true; } -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ void AttPosEKF::ResetPosition(void) { if (staticMode) { @@ -2808,23 +2851,26 @@ void AttPosEKF::ResetPosition(void) // reset the states from the GPS measurements states[7] = posNE[0]; states[8] = posNE[1]; + + // stored horizontal position states to prevent subsequent GPS measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[7][i] = states[7]; + storedStates[8][i] = states[8]; + } } } -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; + + // stored horizontal position states to prevent subsequent Barometer measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[9][i] = states[9]; + } } -/** - * Reset the velocity state. - */ void AttPosEKF::ResetVelocity(void) { if (staticMode) { @@ -2832,10 +2878,16 @@ void AttPosEKF::ResetVelocity(void) states[5] = 0.0f; states[6] = 0.0f; } else if (GPSstatus >= GPS_FIX_3D) { + //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading + + // stored horizontal position states to prevent subsequent GPS measurements from being rejected + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ + storedStates[4][i] = states[4]; + storedStates[5][i] = states[5]; + } } } @@ -2865,8 +2917,8 @@ bool AttPosEKF::StatesNaN() { } // delta velocities // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { if (!isfinite(KH[i][j])) { current_ekf_state.KHNaN = true; @@ -2917,15 +2969,6 @@ out: } -/** - * 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 AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { @@ -2944,9 +2987,6 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) int ret = 0; - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Reset the filter if the states went NaN if (StatesNaN()) { ekf_debug("re-initializing dynamic"); @@ -2965,10 +3005,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); - ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -2982,10 +3022,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); - ResetStoredStates(); ret = 0; } @@ -3155,6 +3195,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z + ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); @@ -3206,8 +3247,8 @@ void AttPosEKF::ZeroVariables() lastVelPosFusion = millis(); // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { KH[i][j] = 0.0f; // intermediate result used for covariance updates KHP[i][j] = 0.0f; // intermediate result used for covariance updates P[i][j] = 0.0f; // covariance matrix @@ -3231,9 +3272,9 @@ void AttPosEKF::ZeroVariables() flowStates[0] = 1.0f; flowStates[1] = 0.0f; - for (unsigned i = 0; i < data_buffer_size; i++) { + for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; i++) { - for (unsigned j = 0; j < n_states; j++) { + for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { storedStates[j][i] = 0.0f; } @@ -3252,11 +3293,11 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) { // Copy states - for (unsigned i = 0; i < n_states; i++) { + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { current_ekf_state.states[i] = states[i]; } - current_ekf_state.n_states = n_states; - current_ekf_state.onGround = onGround; + current_ekf_state.n_states = EKF_STATE_ESTIMATES; + current_ekf_state.onGround = _onGround; current_ekf_state.staticMode = staticMode; current_ekf_state.useCompass = useCompass; current_ekf_state.useAirspeed = useAirspeed; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index b1d71bd74..e15ded977 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -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); - -/** - * 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); + // store staes along with system time stamp in msces + void StoreStates(uint64_t timestamp_ms); -void RecallOmega(float *omegaForFusion, 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 ResetStoredStates(); + void RecallOmega(float *omegaForFusion, uint64_t msec); -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); -void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); + static 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 float sq(float valIn); + static inline float sq(float valIn) {return valIn * valIn;} -static float maxf(float valIn1, float valIn2); + /** + * @brief + * Tell the EKF if the vehicle has landed + **/ + void setOnGround(const bool isLanded); -static float min(float valIn1, float valIn2); + void CovarianceInit(); -void OnGroundCheck(); + void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -void CovarianceInit(); + float ConstrainFloat(float val, float min, float maxf); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + void ConstrainVariances(); -float ConstrainFloat(float val, float min, float maxf); + void ConstrainStates(); -void ConstrainVariances(); + void ForceSymmetry(); -void ConstrainStates(); + /** + * @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 ForceSymmetry(); + /** + * @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(); -int CheckAndBound(struct ekf_status_report *last_error); + /** + * @brief + * Reset the velocity state. + */ + void ResetVelocity(); -void ResetPosition(); + void ZeroVariables(); -void ResetVelocity(); + void GetFilterState(struct ekf_status_report *state); -void ZeroVariables(); + void GetLastErrorState(struct ekf_status_report *last_error); -void GetFilterState(struct ekf_status_report *state); + bool StatesNaN(); -void GetLastErrorState(struct ekf_status_report *last_error); + void InitializeDynamic(float (&initvelNED)[3], float declination); -bool StatesNaN(); +protected: -void InitializeDynamic(float (&initvelNED)[3], float declination); + /** + * @brief + * Initializes algorithm parameters to safe default values + **/ + void InitialiseParameters(); -protected: + void updateDtVelPosFilt(float dt); -void updateDtVelPosFilt(float dt); + bool FilterHealthy(); -bool FilterHealthy(); + bool GyroOffsetsDiverged(); -bool GyroOffsetsDiverged(); + bool VelNEDDiverged(); -bool VelNEDDiverged(); + /** + * @brief + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ + void ResetHeight(); -void ResetHeight(void); + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); -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) }; diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 77cc1eeeb..e2f4c7e82 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -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" diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index a6b670c4d..95b83ead4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -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 |