mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 09:28:58 +00:00
Added local position estimator.
This commit is contained in:
jgoppert
Lorenz Meier
@@ -0,0 +1,310 @@
|
||||
#pragma once
|
||||
|
||||
#include <controllib/uorb/blocks.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#ifdef USE_MATRIX_LIB
|
||||
#include "matrix/src/Matrix.hpp"
|
||||
using namespace matrix;
|
||||
#else
|
||||
#include <Eigen/Eigen>
|
||||
using namespace Eigen;
|
||||
#endif
|
||||
|
||||
// uORB Subscriptions
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
|
||||
// uORB Publications
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/filtered_bottom_flow.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
|
||||
#define CBRK_NO_VISION_KEY 328754
|
||||
|
||||
using namespace control;
|
||||
|
||||
enum fault_t {
|
||||
FAULT_NONE = 0,
|
||||
FAULT_MINOR,
|
||||
FAULT_SEVERE
|
||||
};
|
||||
|
||||
enum sensor_t {
|
||||
SENSOR_BARO = 0,
|
||||
SENSOR_GPS,
|
||||
SENSOR_LIDAR,
|
||||
SENSOR_FLOW,
|
||||
SENSOR_SONAR,
|
||||
SENSOR_VISION,
|
||||
SENSOR_MOCAP
|
||||
};
|
||||
|
||||
class BlockLocalPositionEstimator : public control::SuperBlock
|
||||
{
|
||||
//
|
||||
// The purpose of this estimator is to provide a robust solution for
|
||||
// indoor flight.
|
||||
//
|
||||
// dynamics:
|
||||
//
|
||||
// x(+) = A * x(-) + B * u(+)
|
||||
// y_i = C_i*x
|
||||
//
|
||||
// kalman filter
|
||||
//
|
||||
// E[xx'] = P
|
||||
// E[uu'] = W
|
||||
// E[y_iy_i'] = R_i
|
||||
//
|
||||
// prediction
|
||||
// x(+|-) = A*x(-|-) + B*u(+)
|
||||
// P(+|-) = A*P(-|-)*A' + B*W*B'
|
||||
//
|
||||
// correction
|
||||
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
|
||||
//
|
||||
//
|
||||
// input:
|
||||
// ax, ay, az (acceleration NED)
|
||||
//
|
||||
// states:
|
||||
// px, py, pz , ( position NED)
|
||||
// vx, vy, vz ( vel NED),
|
||||
// bx, by, bz ( TODO accelerometer bias)
|
||||
// tz (TODO terrain altitude)
|
||||
//
|
||||
// measurements:
|
||||
//
|
||||
// sonar: pz (measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// baro: pz
|
||||
//
|
||||
// flow: vx, vy (flow is in body x, y frame)
|
||||
//
|
||||
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
|
||||
//
|
||||
// lidar: px (actual measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// vision: px, py, pz, vx, vy, vz
|
||||
//
|
||||
// mocap: px, py, pz
|
||||
//
|
||||
public:
|
||||
BlockLocalPositionEstimator();
|
||||
void update();
|
||||
virtual ~BlockLocalPositionEstimator();
|
||||
|
||||
private:
|
||||
// prevent copy and assignment
|
||||
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
|
||||
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
|
||||
|
||||
// constants
|
||||
static const uint8_t n_x = 9;
|
||||
static const uint8_t n_u = 3; // 3 accelerations
|
||||
static const uint8_t n_y_flow = 2;
|
||||
static const uint8_t n_y_sonar = 1;
|
||||
static const uint8_t n_y_baro = 1;
|
||||
static const uint8_t n_y_lidar = 1;
|
||||
static const uint8_t n_y_gps = 6;
|
||||
static const uint8_t n_y_vision = 3;
|
||||
static const uint8_t n_y_mocap = 3;
|
||||
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
|
||||
enum {U_ax = 0, U_ay, U_az};
|
||||
enum {Y_baro_z = 0};
|
||||
enum {Y_lidar_z = 0};
|
||||
enum {Y_flow_x = 0, Y_flow_y};
|
||||
enum {Y_sonar_z = 0};
|
||||
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
|
||||
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
|
||||
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
|
||||
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};
|
||||
|
||||
// methods
|
||||
// ----------------------------
|
||||
void initP();
|
||||
|
||||
// predict the next state
|
||||
void predict();
|
||||
|
||||
// correct the state prediction wtih a measurement
|
||||
void correctBaro();
|
||||
void correctGps();
|
||||
void correctLidar();
|
||||
void correctFlow();
|
||||
void correctSonar();
|
||||
void correctVision();
|
||||
void correctmocap();
|
||||
|
||||
// sensor initialization
|
||||
void updateHome();
|
||||
void initBaro();
|
||||
void initGps();
|
||||
void initLidar();
|
||||
void initSonar();
|
||||
void initFlow();
|
||||
void initVision();
|
||||
void initmocap();
|
||||
|
||||
// publications
|
||||
void publishLocalPos();
|
||||
void publishGlobalPos();
|
||||
void publishFilteredFlow();
|
||||
void publishEstimatorStatus();
|
||||
|
||||
// attributes
|
||||
// ----------------------------
|
||||
|
||||
// subscriptions
|
||||
uORB::Subscription<vehicle_status_s> _sub_status;
|
||||
uORB::Subscription<actuator_armed_s> _sub_armed;
|
||||
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
|
||||
uORB::Subscription<vehicle_attitude_s> _sub_att;
|
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
|
||||
uORB::Subscription<optical_flow_s> _sub_flow;
|
||||
uORB::Subscription<sensor_combined_s> _sub_sensor;
|
||||
uORB::Subscription<distance_sensor_s> _sub_distance;
|
||||
uORB::Subscription<parameter_update_s> _sub_param_update;
|
||||
uORB::Subscription<manual_control_setpoint_s> _sub_manual;
|
||||
uORB::Subscription<home_position_s> _sub_home;
|
||||
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
|
||||
uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
|
||||
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
|
||||
|
||||
// publications
|
||||
uORB::Publication<vehicle_local_position_s> _pub_lpos;
|
||||
uORB::Publication<vehicle_global_position_s> _pub_gpos;
|
||||
uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
|
||||
uORB::Publication<estimator_status_s> _pub_est_status;
|
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
// parameters
|
||||
BlockParamInt _integrate;
|
||||
|
||||
BlockParamFloat _flow_xy_stddev;
|
||||
BlockParamFloat _sonar_z_stddev;
|
||||
|
||||
BlockParamFloat _lidar_z_stddev;
|
||||
|
||||
BlockParamFloat _accel_xy_stddev;
|
||||
BlockParamFloat _accel_z_stddev;
|
||||
|
||||
BlockParamFloat _baro_stddev;
|
||||
|
||||
BlockParamFloat _gps_xy_stddev;
|
||||
BlockParamFloat _gps_z_stddev;
|
||||
|
||||
BlockParamFloat _gps_vxy_stddev;
|
||||
BlockParamFloat _gps_vz_stddev;
|
||||
|
||||
BlockParamFloat _gps_eph_max;
|
||||
|
||||
BlockParamFloat _vision_xy_stddev;
|
||||
BlockParamFloat _vision_z_stddev;
|
||||
BlockParamInt _no_vision;
|
||||
BlockParamFloat _beta_max;
|
||||
|
||||
BlockParamFloat _mocap_p_stddev;
|
||||
|
||||
// process noise
|
||||
BlockParamFloat _pn_p_noise_power;
|
||||
BlockParamFloat _pn_v_noise_power;
|
||||
BlockParamFloat _pn_b_noise_power;
|
||||
|
||||
// misc
|
||||
struct pollfd _polls[3];
|
||||
uint64_t _timeStamp;
|
||||
uint64_t _time_last_xy;
|
||||
uint64_t _time_last_flow;
|
||||
uint64_t _time_last_baro;
|
||||
uint64_t _time_last_gps;
|
||||
uint64_t _time_last_lidar;
|
||||
uint64_t _time_last_sonar;
|
||||
uint64_t _time_last_vision_p;
|
||||
uint64_t _time_last_mocap;
|
||||
int _mavlink_fd;
|
||||
|
||||
// initialization flags
|
||||
bool _baroInitialized;
|
||||
bool _gpsInitialized;
|
||||
bool _lidarInitialized;
|
||||
bool _sonarInitialized;
|
||||
bool _flowInitialized;
|
||||
bool _visionInitialized;
|
||||
bool _mocapInitialized;
|
||||
|
||||
// init counts
|
||||
int _baroInitCount;
|
||||
int _gpsInitCount;
|
||||
int _lidarInitCount;
|
||||
int _sonarInitCount;
|
||||
int _flowInitCount;
|
||||
int _visionInitCount;
|
||||
int _mocapInitCount;
|
||||
|
||||
// reference altitudes
|
||||
float _altHome;
|
||||
bool _altHomeInitialized;
|
||||
float _baroAltHome;
|
||||
float _gpsAltHome;
|
||||
float _lidarAltHome;
|
||||
float _sonarAltHome;
|
||||
float _flowAltHome;
|
||||
Vector3f _visionHome;
|
||||
Vector3f _mocapHome;
|
||||
|
||||
// flow integration
|
||||
float _flowX;
|
||||
float _flowY;
|
||||
float _flowMeanQual;
|
||||
|
||||
// referene lat/lon
|
||||
double _gpsLatHome;
|
||||
double _gpsLonHome;
|
||||
|
||||
// status
|
||||
bool _canEstimateXY;
|
||||
bool _canEstimateZ;
|
||||
bool _xyTimeout;
|
||||
|
||||
// sensor faults
|
||||
fault_t _baroFault;
|
||||
fault_t _gpsFault;
|
||||
fault_t _lidarFault;
|
||||
fault_t _flowFault;
|
||||
fault_t _sonarFault;
|
||||
fault_t _visionFault;
|
||||
fault_t _mocapFault;
|
||||
|
||||
bool _visionTimeout;
|
||||
bool _mocapTimeout;
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
perf_counter_t _err_perf;
|
||||
|
||||
// state space
|
||||
Matrix<float, n_x, 1> _x; // state vector
|
||||
Matrix<float, n_u, 1> _u; // input vector
|
||||
Matrix<float, n_x, n_x> _P; // state covariance matrix
|
||||
};
|
||||
Reference in New Issue
Block a user