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

EKF: move all estimator specific types to common header

Browse Source
This commit is contained in:
bugobliterator 2016-01-30 12:44:19 -08:00
parent b05e2d825c
commit 287f5eb0c2
3 changed files with 211 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

206
EKF/common.h Normal file
View File

@ -0,0 +1,206 @@
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 estimator_base.h
* Definition of base class for attitude estimators
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
*
*/
namespace estimator {
struct gps_message {
uint64_t time_usec;
int32_t lat; // Latitude in 1E-7 degrees
int32_t lon; // Longitude in 1E-7 degrees
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
float eph; // GPS horizontal position accuracy in m
float epv; // GPS vertical position accuracy in m
float sacc; // GPS speed accuracy in m/s
uint64_t time_usec_vel; // Timestamp for velocity informations
float vel_m_s; // GPS ground speed (m/s)
float vel_ned[3]; // GPS ground speed NED
bool vel_ned_valid; // GPS ground speed is valid
uint8_t nsats; // number of satellites used
float gdop; // geometric dilution of precision
};
typedef matrix::Vector<float, 2> Vector2f;
typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> Matrix3f;
struct outputSample {
Quaternion quat_nominal;
Vector3f vel;
Vector3f pos;
uint64_t time_us;
};
struct imuSample {
Vector3f delta_ang;
Vector3f delta_vel;
float delta_ang_dt;
float delta_vel_dt;
uint64_t time_us;
};
struct gpsSample {
Vector2f pos;
float hgt;
Vector3f vel;
uint64_t time_us;
};
struct magSample {
Vector3f mag;
uint64_t time_us;
};
struct baroSample {
float hgt;
uint64_t time_us;
};
struct rangeSample {
float rng;
uint64_t time_us;
};
struct airspeedSample {
float airspeed;
uint64_t time_us;
};
struct flowSample {
Vector2f flowRadXY;
Vector2f flowRadXYcomp;
uint64_t time_us;
};
struct parameters {
float mag_delay_ms = 0.0f;
float baro_delay_ms = 0.0f;
float gps_delay_ms = 200.0f;
float airspeed_delay_ms = 200.0f;
// input noise
float gyro_noise = 0.001f;
float accel_noise = 0.1f;
// process noise
float gyro_bias_p_noise = 1e-5f;
float accel_bias_p_noise = 1e-3f;
float gyro_scale_p_noise = 1e-4f;
float mag_p_noise = 1e-2f;
float wind_vel_p_noise = 0.05f;
float gps_vel_noise = 0.05f;
float gps_pos_noise = 1.0f;
float baro_noise = 0.1f;
float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float req_hacc = 5.0f; // maximum acceptable horizontal position error
float req_vacc = 8.0f; // maximum acceptable vertical position error
float req_sacc = 1.0f; // maximum acceptable speed error
int req_nsats = 6; // minimum acceptable satellite count
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
};
struct stateSample {
Vector3f ang_error;
Vector3f vel;
Vector3f pos;
Vector3f gyro_bias;
Vector3f gyro_scale;
float accel_z_bias;
Vector3f mag_I;
Vector3f mag_B;
Vector2f wind_vel;
Quaternion quat_nominal;
};
struct fault_status_t {
bool bad_mag_x: 1;
bool bad_mag_y: 1;
bool bad_mag_z: 1;
bool bad_airspeed: 1;
bool bad_sideslip: 1;
};
// publish the status of various GPS quality checks
union gps_check_fail_status_u {
struct {
uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution)
uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive
} flags;
uint16_t value;
};
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
} flags;
uint16_t value;
};
}

32
EKF/ekf.h
View File

@ -84,20 +84,6 @@ public:
bool collect_gps(uint64_t time_usec, struct gps_message *gps);
bool collect_imu(imuSample &imu);
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
} flags;
uint16_t value;
};
filter_control_status_u _control_status={};
// get the ekf WGS-84 origin positoin and height and the system time it was last set
@ -157,22 +143,8 @@ private:
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
// publish the status of various GPS quality checks
union gps_check_fail_status_u {
struct {
uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution)
uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient
uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive
} flags;
uint16_t value;
}_gps_check_fail_status;
gps_check_fail_status_u _gps_check_fail_status;
void calculateOutputStates();

138
EKF/estimator_base.h
View File

@ -43,118 +43,8 @@
#include <matrix/matrix/math.hpp>
#include <lib/geo/geo.h>
#include "RingBuffer.h"
namespace estimator {
struct gps_message {
uint64_t time_usec;
int32_t lat; // Latitude in 1E-7 degrees
int32_t lon; // Longitude in 1E-7 degrees
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
float eph; // GPS horizontal position accuracy in m
float epv; // GPS vertical position accuracy in m
float sacc; // GPS speed accuracy in m/s
uint64_t time_usec_vel; // Timestamp for velocity informations
float vel_m_s; // GPS ground speed (m/s)
float vel_ned[3]; // GPS ground speed NED
bool vel_ned_valid; // GPS ground speed is valid
uint8_t nsats; // number of satellites used
float gdop; // geometric dilution of precision
};
typedef matrix::Vector<float, 2> Vector2f;
typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> Matrix3f;
struct outputSample {
Quaternion quat_nominal;
Vector3f vel;
Vector3f pos;
uint64_t time_us;
};
struct imuSample {
Vector3f delta_ang;
Vector3f delta_vel;
float delta_ang_dt;
float delta_vel_dt;
uint64_t time_us;
};
struct gpsSample {
Vector2f pos;
float hgt;
Vector3f vel;
uint64_t time_us;
};
struct magSample {
Vector3f mag;
uint64_t time_us;
};
struct baroSample {
float hgt;
uint64_t time_us;
};
struct rangeSample {
float rng;
uint64_t time_us;
};
struct airspeedSample {
float airspeed;
uint64_t time_us;
};
struct flowSample {
Vector2f flowRadXY;
Vector2f flowRadXYcomp;
uint64_t time_us;
};
struct parameters {
float mag_delay_ms = 0.0f;
float baro_delay_ms = 0.0f;
float gps_delay_ms = 200.0f;
float airspeed_delay_ms = 200.0f;
// input noise
float gyro_noise = 0.001f;
float accel_noise = 0.1f;
// process noise
float gyro_bias_p_noise = 1e-5f;
float accel_bias_p_noise = 1e-3f;
float gyro_scale_p_noise = 1e-4f;
float mag_p_noise = 1e-2f;
float wind_vel_p_noise = 0.05f;
float gps_vel_noise = 0.05f;
float gps_pos_noise = 1.0f;
float baro_noise = 0.1f;
float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
float mag_declination_deg = 0.0f; // magnetic declination in degrees
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
float req_hacc = 5.0f; // maximum acceptable horizontal position error
float req_vacc = 8.0f; // maximum acceptable vertical position error
float req_sacc = 1.0f; // maximum acceptable speed error
int req_nsats = 6; // minimum acceptable satellite count
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
};
}
#include "common.h"
using namespace estimator;
class EstimatorBase
@ -277,19 +167,6 @@ public:
protected:
struct stateSample {
Vector3f ang_error;
Vector3f vel;
Vector3f pos;
Vector3f gyro_bias;
Vector3f gyro_scale;
float accel_z_bias;
Vector3f mag_I;
Vector3f mag_B;
Vector2f wind_vel;
Quaternion quat_nominal;
} _state;
parameters _params; // filter parameters
static const uint8_t OBS_BUFFER_LENGTH = 10;
@ -346,16 +223,7 @@ protected:
uint64_t _time_last_range;
uint64_t _time_last_airspeed;
// flags capturing information about severe nummerical problems for various fusions
struct {
bool bad_mag_x: 1;
bool bad_mag_y: 1;
bool bad_mag_z: 1;
bool bad_airspeed: 1;
bool bad_sideslip: 1;
} _fault_status;
// initialise variables to default startup values and set time stamps to specified value
void initialiseVariables(uint64_t timestamp);
fault_status_t _fault_status;
bool initialise_interface(uint64_t timestamp);
};