mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
delete exampales ekf_att_pos_estimator
This commit is contained in:
Daniel Agar
@@ -190,7 +190,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
|
||||
# EKF
|
||||
#examples/ekf_att_pos_estimator
|
||||
)
|
||||
@@ -206,7 +206,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
examples/hwtest
|
||||
|
||||
# EKF
|
||||
examples/ekf_att_pos_estimator
|
||||
)
|
||||
@@ -205,7 +205,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
|
||||
# EKF
|
||||
#examples/ekf_att_pos_estimator
|
||||
)
|
||||
|
||||
@@ -209,7 +209,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
examples/hwtest
|
||||
|
||||
# EKF
|
||||
examples/ekf_att_pos_estimator
|
||||
)
|
||||
|
||||
@@ -202,7 +202,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
examples/hwtest
|
||||
|
||||
# EKF
|
||||
examples/ekf_att_pos_estimator
|
||||
)
|
||||
@@ -201,7 +201,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
|
||||
# EKF
|
||||
#examples/ekf_att_pos_estimator
|
||||
)
|
||||
@@ -202,7 +202,4 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
|
||||
# EKF
|
||||
#examples/ekf_att_pos_estimator
|
||||
)
|
||||
@@ -182,9 +182,6 @@ set(config_module_list
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
|
||||
# EKF
|
||||
examples/ekf_att_pos_estimator
|
||||
)
|
||||
|
||||
# Default config_sitl_rcS_dir (posix_sitl_default), this is overwritten later
|
||||
|
||||
@@ -1,369 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_status.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/actuator_controls.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 <uORB/topics/distance_sensor.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 <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
|
||||
#include <geo/geo.h>
|
||||
#include <terrain_estimation/terrain_estimator.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include "estimator_22states.h"
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
//Forward declaration
|
||||
class AttPosEKF;
|
||||
|
||||
class AttitudePositionEstimatorEKF : public control::SuperBlock
|
||||
{
|
||||
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; }
|
||||
|
||||
static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
|
||||
|
||||
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 _vehicle_status_sub;
|
||||
int _vehicle_land_detected_sub;
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_control_sub; /**< notification of manual control updates */
|
||||
int _home_sub; /**< home position as defined by commander / user */
|
||||
int _landDetectorSub;
|
||||
int _armedSub;
|
||||
|
||||
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 _vehicle_status;
|
||||
struct vehicle_land_detected_s _vehicle_land_detected;
|
||||
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 distance_sensor_s _distance; /**< distance estimate */
|
||||
struct actuator_armed_s _armed;
|
||||
|
||||
hrt_abstime _last_accel;
|
||||
hrt_abstime _last_mag;
|
||||
unsigned _prediction_steps;
|
||||
uint64_t _prediction_last;
|
||||
|
||||
struct sensor_combined_s _sensor_combined;
|
||||
|
||||
struct map_projection_reference_s _pos_ref;
|
||||
|
||||
float _filter_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 _distance_last_valid;
|
||||
bool _data_good; ///< all required filter data is ok
|
||||
bool _ekf_logging; ///< log EKF state
|
||||
unsigned _debug; ///< debug level - default 0
|
||||
bool _was_landed; ///< indicates if was landed in previous iteration
|
||||
|
||||
bool _newHgtData;
|
||||
bool _newAdsData;
|
||||
bool _newDataMag;
|
||||
bool _newRangeData;
|
||||
|
||||
orb_advert_t _mavlink_log_pub;
|
||||
|
||||
control::BlockParamFloat _mag_offset_x;
|
||||
control::BlockParamFloat _mag_offset_y;
|
||||
control::BlockParamFloat _mag_offset_z;
|
||||
|
||||
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;
|
||||
int32_t airspeed_disabled;
|
||||
} _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;
|
||||
param_t airspeed_disabled;
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
AttPosEKF *_ekf;
|
||||
|
||||
TerrainEstimator *_terrain_estimator;
|
||||
|
||||
/* Low pass filter for attitude rates */
|
||||
math::LowPassFilter2p _LP_att_P;
|
||||
math::LowPassFilter2p _LP_att_Q;
|
||||
math::LowPassFilter2p _LP_att_R;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
int parameters_update();
|
||||
|
||||
/**
|
||||
* Update control outputs
|
||||
*
|
||||
*/
|
||||
void control_update();
|
||||
|
||||
/**
|
||||
* Check for changes in land detected.
|
||||
*/
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Check for changes in land detected.
|
||||
*/
|
||||
void vehicle_land_detected_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();
|
||||
|
||||
/**
|
||||
* Initialize the reference position for the local coordinate frame
|
||||
*/
|
||||
void initReferencePosition(hrt_abstime timestamp, bool gps_valid,
|
||||
double lat, double lon, float gps_alt, float baro_alt);
|
||||
|
||||
/**
|
||||
* @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();
|
||||
};
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE examples__ekf_att_pos_estimator
|
||||
MAIN ekf_att_pos_estimator
|
||||
COMPILE_FLAGS
|
||||
STACK_MAIN 3000
|
||||
STACK_MAX 3400
|
||||
SRCS
|
||||
ekf_att_pos_estimator_main.cpp
|
||||
estimator_22states.cpp
|
||||
estimator_utilities.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,307 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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 ekf_att_pos_estimator_params.c
|
||||
*
|
||||
* Parameters defined by the attitude and position estimator task
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
/*
|
||||
* Estimator parameters, accessible via MAVLink
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* Velocity estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the velocity estimate from GPS.
|
||||
*
|
||||
* @unit ms
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
|
||||
|
||||
/**
|
||||
* Position estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the position estimate from GPS.
|
||||
*
|
||||
* @unit ms
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
|
||||
|
||||
/**
|
||||
* Height estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the height estimate from the barometer.
|
||||
*
|
||||
* @unit ms
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
|
||||
|
||||
/**
|
||||
* Mag estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the magnetic field estimate from
|
||||
* the magnetometer.
|
||||
*
|
||||
* @unit ms
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
|
||||
|
||||
/**
|
||||
* True airspeeed estimate delay
|
||||
*
|
||||
* The delay in milliseconds of the airspeed estimate.
|
||||
*
|
||||
* @unit ms
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
|
||||
|
||||
/**
|
||||
* GPS vs. barometric altitude update weight
|
||||
*
|
||||
* RE-CHECK this.
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
|
||||
|
||||
/**
|
||||
* Airspeed measurement noise.
|
||||
*
|
||||
* Increasing this value will make the filter trust this sensor
|
||||
* less and trust other sensors more.
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 5.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
|
||||
|
||||
/**
|
||||
* Velocity measurement noise in north-east (horizontal) direction.
|
||||
*
|
||||
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
|
||||
*
|
||||
* @min 0.05
|
||||
* @max 5.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
|
||||
|
||||
/**
|
||||
* Velocity noise in down (vertical) direction
|
||||
*
|
||||
* Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7
|
||||
*
|
||||
* @min 0.2
|
||||
* @max 3.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f);
|
||||
|
||||
/**
|
||||
* Position noise in north-east (horizontal) direction
|
||||
*
|
||||
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
|
||||
*
|
||||
* @min 0.1
|
||||
* @max 10.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
|
||||
|
||||
/**
|
||||
* Position noise in down (vertical) direction
|
||||
*
|
||||
* Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 3.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f);
|
||||
|
||||
/**
|
||||
* Magnetometer measurement noise
|
||||
*
|
||||
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 1.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
|
||||
|
||||
/**
|
||||
* Gyro process noise
|
||||
*
|
||||
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
|
||||
* This noise controls how much the filter trusts the gyro measurements.
|
||||
* Increasing it makes the filter trust the gyro less and other sensors more.
|
||||
*
|
||||
* @min 0.001
|
||||
* @max 0.05
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
|
||||
|
||||
/**
|
||||
* Accelerometer process noise
|
||||
*
|
||||
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
|
||||
* Increasing this value makes the filter trust the accelerometer less
|
||||
* and other sensors more.
|
||||
*
|
||||
* @min 0.05
|
||||
* @max 1.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f);
|
||||
|
||||
/**
|
||||
* Gyro bias estimate process noise
|
||||
*
|
||||
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
|
||||
* Increasing this value will make the gyro bias converge faster but noisier.
|
||||
*
|
||||
* @min 0.00000005
|
||||
* @max 0.00001
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
|
||||
|
||||
/**
|
||||
* Accelerometer bias estimate process noise
|
||||
*
|
||||
* Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f.
|
||||
* Increasing this value makes the bias estimation faster and noisier.
|
||||
*
|
||||
* @min 0.00001
|
||||
* @max 0.001
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f);
|
||||
|
||||
/**
|
||||
* Magnetometer earth frame offsets process noise
|
||||
*
|
||||
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
|
||||
* Increasing this value makes the magnetometer earth bias estimate converge
|
||||
* faster but also noisier.
|
||||
*
|
||||
* @min 0.0001
|
||||
* @max 0.01
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
|
||||
|
||||
/**
|
||||
* Magnetometer body frame offsets process noise
|
||||
*
|
||||
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
|
||||
* Increasing this value makes the magnetometer body bias estimate converge faster
|
||||
* but also noisier.
|
||||
*
|
||||
* @min 0.0001
|
||||
* @max 0.01
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
|
||||
|
||||
/**
|
||||
* Magnetometer X bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
|
||||
|
||||
/**
|
||||
* Magnetometer Y bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
|
||||
|
||||
/**
|
||||
* Magnetometer Z bias
|
||||
*
|
||||
* The magnetometer bias. This bias is learnt by the filter over time and
|
||||
* persists between boots.
|
||||
*
|
||||
* @min -0.6
|
||||
* @max 0.6
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
|
||||
|
||||
/**
|
||||
* Threshold for filter initialization.
|
||||
*
|
||||
* If the standard deviation of the GPS position estimate is below this threshold
|
||||
* in meters, the filter will initialize.
|
||||
*
|
||||
* @min 0.3
|
||||
* @max 10.0
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
|
||||
File diff suppressed because one or more lines are too long
@@ -1,424 +0,0 @@
|
||||
/****************************************************************************
|
||||
* 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>
|
||||
|
||||
constexpr size_t EKF_STATE_ESTIMATES = 22;
|
||||
constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
|
||||
|
||||
/* ##############################################
|
||||
*
|
||||
* M A I N F I L T E R P A R A M E T E R S
|
||||
*
|
||||
* ########################################### */
|
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in
|
||||
* the InitialiseParameters()
|
||||
*/
|
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
float dVelBiasSigma;
|
||||
float magEarthSigma;
|
||||
float magBodySigma;
|
||||
float gndHgtSigma;
|
||||
|
||||
float vneSigma;
|
||||
float vdSigma;
|
||||
float posNeSigma;
|
||||
float posDSigma;
|
||||
float magMeasurementSigma;
|
||||
float airspeedMeasurementSigma;
|
||||
|
||||
float gyroProcessNoise;
|
||||
float accelProcessNoise;
|
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
struct mag_state_struct {
|
||||
unsigned obsIndex;
|
||||
float MagPred[3];
|
||||
float SH_MAG[9];
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
float magN;
|
||||
float magE;
|
||||
float magD;
|
||||
float magXbias;
|
||||
float magYbias;
|
||||
float magZbias;
|
||||
float R_MAG;
|
||||
Mat3f DCM;
|
||||
};
|
||||
|
||||
struct mag_state_struct magstate;
|
||||
struct mag_state_struct resetMagState;
|
||||
|
||||
|
||||
|
||||
|
||||
// Global variables
|
||||
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[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)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f prevDelAng; ///< previous delta angle, used for coning correction
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f lastGyroOffset; // Last gyro offset
|
||||
Vector3f delAngTotal;
|
||||
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
|
||||
Mat3f Tnb; // transformation amtrix from NED to body coordinates
|
||||
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
|
||||
float dtIMUfilt; // average time between IMU measurements (sec)
|
||||
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
|
||||
float rngMea; // Ground distance
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
|
||||
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
float innovVtas; // innovation output
|
||||
float innovRng; ///< Range finder innovation
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
|
||||
float varInnovVtas; // innovation variance output
|
||||
float varInnovRng; // range finder innovation variance
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination; ///< magnetic declination
|
||||
double latRef; // WGS-84 latitude of reference point (rad)
|
||||
double lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
bool refSet; ///< flag to indicate if the reference position has been set
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
|
||||
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
|
||||
|
||||
// GPS input data variables
|
||||
double gpsLat;
|
||||
double gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised;
|
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
bool fuseRngData; ///< true when range data is fused
|
||||
bool fuseOptFlowData; // true when optical flow data is fused
|
||||
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
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 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
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
bool useOpticalFlow; // true when optical flow data is being used
|
||||
|
||||
bool ekfDiverged;
|
||||
uint64_t lastReset;
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection;
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
// Optical Flow error estimation
|
||||
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
|
||||
float flowStates[2]; // flow states [scale factor, terrain position]
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
|
||||
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
|
||||
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
|
||||
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
|
||||
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
|
||||
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
|
||||
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
|
||||
float flowInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
float rngInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
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 updateDtHgtFilt(float dt);
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
||||
void FuseVelposNED();
|
||||
|
||||
void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
/**
|
||||
* @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)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], 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]);
|
||||
|
||||
// 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
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
* time-wise where valid states were updated and invalid remained at the old
|
||||
* value.
|
||||
*/
|
||||
int RecallStates(float *statesForFusion, uint64_t msec);
|
||||
|
||||
void RecallOmega(float *omegaForFusion, uint64_t msec);
|
||||
|
||||
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
//static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static inline float sq(float valIn) {return valIn * valIn;}
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Tell the EKF if the vehicle has landed
|
||||
**/
|
||||
void setOnGround(const bool isLanded);
|
||||
|
||||
void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float maxf);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
void ConstrainStates();
|
||||
|
||||
void ForceSymmetry();
|
||||
|
||||
/**
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* @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();
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Reset the velocity state.
|
||||
*/
|
||||
void ResetVelocity();
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state);
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN();
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Tells the EKF wheter the vehicle is a fixed wing frame or not.
|
||||
* This changes the way the EKF fuses position or attitude calulations
|
||||
* by making some assumptions on movement.
|
||||
* @param fixedWing
|
||||
* true if the vehicle moves like a Fixed Wing, false otherwise
|
||||
**/
|
||||
void setIsFixedWing(const bool fixedWing);
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Reset internal filter states and clear all variables to zero value
|
||||
*/
|
||||
void ZeroVariables();
|
||||
|
||||
void get_covariance(float c[28]);
|
||||
|
||||
float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Initializes algorithm parameters to safe default values
|
||||
**/
|
||||
void InitialiseParameters();
|
||||
|
||||
void updateDtVelPosFilt(float dt);
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
bool GyroOffsetsDiverged();
|
||||
|
||||
bool VelNEDDiverged();
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* Reset the height state.
|
||||
*
|
||||
* This resets the height state with the last altitude measurements
|
||||
*/
|
||||
void ResetHeight();
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
private:
|
||||
bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type
|
||||
bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
uint64_t getMicros();
|
||||
|
||||
@@ -1,235 +0,0 @@
|
||||
/****************************************************************************
|
||||
* 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"
|
||||
|
||||
// Define EKF_DEBUG here to enable the debug print calls
|
||||
// if the macro is not set, these will be completely
|
||||
// optimized out by the compiler.
|
||||
//#define EKF_DEBUG
|
||||
|
||||
#ifdef EKF_DEBUG
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
static void
|
||||
ekf_debug_print(const char *fmt, va_list args)
|
||||
{
|
||||
fprintf(stderr, "%s: ", "[ekf]");
|
||||
vfprintf(stderr, fmt, args);
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
}
|
||||
|
||||
void
|
||||
ekf_debug(const char *fmt, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start(args, fmt);
|
||||
ekf_debug_print(fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void ekf_debug(const char *fmt, ...) { while(0){} }
|
||||
#endif
|
||||
|
||||
/* we don't want to pull in the standard lib just to swap two floats */
|
||||
void swap_var(float &d1, float &d2);
|
||||
|
||||
float Vector3f::length() const
|
||||
{
|
||||
return sqrtf(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
void Vector3f::zero()
|
||||
{
|
||||
x = 0.0f;
|
||||
y = 0.0f;
|
||||
z = 0.0f;
|
||||
}
|
||||
|
||||
Mat3f::Mat3f() :
|
||||
x{1.0f, 0.0f, 0.0f},
|
||||
y{0.0f, 1.0f, 0.0f},
|
||||
z{0.0f, 0.0f, 1.0f}
|
||||
{
|
||||
}
|
||||
|
||||
void Mat3f::identity() {
|
||||
x.x = 1.0f;
|
||||
x.y = 0.0f;
|
||||
x.z = 0.0f;
|
||||
|
||||
y.x = 0.0f;
|
||||
y.y = 1.0f;
|
||||
y.z = 0.0f;
|
||||
|
||||
z.x = 0.0f;
|
||||
z.y = 0.0f;
|
||||
z.z = 1.0f;
|
||||
}
|
||||
|
||||
Mat3f Mat3f::transpose() const
|
||||
{
|
||||
Mat3f ret = *this;
|
||||
swap_var(ret.x.y, ret.y.x);
|
||||
swap_var(ret.x.z, ret.z.x);
|
||||
swap_var(ret.y.z, ret.z.y);
|
||||
return ret;
|
||||
}
|
||||
|
||||
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
|
||||
{
|
||||
velNEDr[0] = gpsGndSpd*cosf(gpsCourse);
|
||||
velNEDr[1] = gpsGndSpd*sinf(gpsCourse);
|
||||
velNEDr[2] = gpsVelD;
|
||||
}
|
||||
|
||||
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
|
||||
{
|
||||
posNEDr[0] = earthRadius * (lat - latReference);
|
||||
posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
|
||||
posNEDr[2] = -(hgt - hgtReference);
|
||||
}
|
||||
|
||||
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
|
||||
{
|
||||
lat = latRef + (double)posNEDi[0] * earthRadiusInv;
|
||||
lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef);
|
||||
hgt = hgtRef - posNEDi[2];
|
||||
}
|
||||
|
||||
// overload + operator to provide a vector addition
|
||||
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x + vecIn2.x;
|
||||
vecOut.y = vecIn1.y + vecIn2.y;
|
||||
vecOut.z = vecIn1.z + vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload - operator to provide a vector subtraction
|
||||
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x - vecIn2.x;
|
||||
vecOut.y = vecIn1.y - vecIn2.y;
|
||||
vecOut.z = vecIn1.z - vecIn2.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a matrix vector product
|
||||
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
|
||||
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
|
||||
vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a matrix product
|
||||
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
|
||||
{
|
||||
Mat3f matOut;
|
||||
matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
|
||||
matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
|
||||
matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
|
||||
|
||||
matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
|
||||
matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
|
||||
matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
|
||||
|
||||
matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
|
||||
matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
|
||||
matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
|
||||
|
||||
return matOut;
|
||||
}
|
||||
|
||||
// overload % operator to provide a vector cross product
|
||||
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
|
||||
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
|
||||
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a vector scaler product
|
||||
Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vecIn1.x * sclIn1;
|
||||
vecOut.y = vecIn1.y * sclIn1;
|
||||
vecOut.z = vecIn1.z * sclIn1;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload / operator to provide a vector scalar division
|
||||
Vector3f operator/(const Vector3f &vec, const float scalar)
|
||||
{
|
||||
Vector3f vecOut;
|
||||
vecOut.x = vec.x / scalar;
|
||||
vecOut.y = vec.y / scalar;
|
||||
vecOut.z = vec.z / scalar;
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
void swap_var(float &d1, float &d2)
|
||||
{
|
||||
float tmp = d1;
|
||||
d1 = d2;
|
||||
d2 = tmp;
|
||||
}
|
||||
@@ -1,135 +0,0 @@
|
||||
/****************************************************************************
|
||||
* 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
|
||||
#define earthRate 0.000072921f
|
||||
#define earthRadius 6378145.0
|
||||
#define earthRadiusInv 1.5678540e-7
|
||||
|
||||
class Vector3f
|
||||
{
|
||||
public:
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) :
|
||||
x(a),
|
||||
y(b),
|
||||
z(c)
|
||||
{}
|
||||
|
||||
float length() const;
|
||||
void zero();
|
||||
};
|
||||
|
||||
class Mat3f
|
||||
{
|
||||
private:
|
||||
public:
|
||||
Vector3f x;
|
||||
Vector3f y;
|
||||
Vector3f z;
|
||||
|
||||
Mat3f();
|
||||
|
||||
void identity();
|
||||
Mat3f transpose() const;
|
||||
};
|
||||
|
||||
Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
|
||||
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
|
||||
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
|
||||
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
|
||||
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
|
||||
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
|
||||
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
|
||||
Vector3f operator/(const Vector3f &vec, const float scalar);
|
||||
|
||||
enum GPS_FIX {
|
||||
GPS_FIX_NOFIX = 0,
|
||||
GPS_FIX_2D = 2,
|
||||
GPS_FIX_3D = 3
|
||||
};
|
||||
|
||||
struct ekf_status_report {
|
||||
bool error;
|
||||
bool velHealth;
|
||||
bool posHealth;
|
||||
bool hgtHealth;
|
||||
bool velTimeout;
|
||||
bool posTimeout;
|
||||
bool hgtTimeout;
|
||||
bool imuTimeout;
|
||||
bool onGround;
|
||||
bool staticMode;
|
||||
bool useCompass;
|
||||
bool useAirspeed;
|
||||
uint32_t velFailTime;
|
||||
uint32_t posFailTime;
|
||||
uint32_t hgtFailTime;
|
||||
float states[32];
|
||||
unsigned n_states;
|
||||
bool angNaN;
|
||||
bool summedDelVelNaN;
|
||||
bool KHNaN;
|
||||
bool KHPNaN;
|
||||
bool PNaN;
|
||||
bool covarianceNaN;
|
||||
bool kalmanGainsNaN;
|
||||
bool statesNaN;
|
||||
bool gyroOffsetsExcessive;
|
||||
bool covariancesExcessive;
|
||||
bool velOffsetExcessive;
|
||||
};
|
||||
|
||||
void ekf_debug(const char *fmt, ...);
|
||||
|
||||
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference);
|
||||
|
||||
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
|
||||
Reference in New Issue
Block a user