Skip to content

Commit

Permalink
WIP: EKF use ecl_float_t type (double precision floating point where …
Browse files Browse the repository at this point in the history
…available)
  • Loading branch information
dagar committed Jun 8, 2019
1 parent 8a0beb8 commit 5dff9e7
Show file tree
Hide file tree
Showing 3 changed files with 67 additions and 48 deletions.
2 changes: 1 addition & 1 deletion src/lib/ecl
Submodule ecl updated from 54ac14 to bd1df8
6 changes: 4 additions & 2 deletions src/modules/ekf2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,10 @@ px4_add_module(
MODULE modules__ekf2
MAIN ekf2
COMPILE_FLAGS
STACK_MAIN 2500
STACK_MAX 4000
-Wno-narrowing
-Wno-double-promotion
STACK_MAIN 4000
STACK_MAX 4700
SRCS
ekf2_main.cpp
DEPENDS
Expand Down
107 changes: 62 additions & 45 deletions src/modules/ekf2/ekf2_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,10 @@
using math::constrain;
using namespace time_literals;

using matrix::Vector2f;
using matrix::Vector3f;
using matrix::Quatf;

extern "C" __EXPORT int ekf2_main(int argc, char *argv[]);

class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
Expand Down Expand Up @@ -231,9 +235,9 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity

// GPS blending and switching
gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
estimator::gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
estimator::gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
estimator::gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
Vector3f _blended_antenna_offset = {}; ///< blended antenna offset
Expand Down Expand Up @@ -288,9 +292,9 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub;

Ekf _ekf;
estimator::Ekf _ekf;

parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
estimator::parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)

DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
Expand Down Expand Up @@ -432,26 +436,26 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)

// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
(ParamFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamFloat<px4::params::EKF2_OF_POS_X>)
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
(ParamFloat<px4::params::EKF2_OF_POS_Y>)
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
(ParamFloat<px4::params::EKF2_OF_POS_Z>)
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
(ParamFloat<px4::params::EKF2_EV_POS_X>)
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
(ParamFloat<px4::params::EKF2_EV_POS_Y>)
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
(ParamFloat<px4::params::EKF2_EV_POS_Z>)
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)

// control of airspeed and sideslip fusion
Expand Down Expand Up @@ -604,21 +608,6 @@ Ekf2::Ekf2():
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
_param_ekf2_of_gate(_params->flow_innov_gate),
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
_param_ekf2_tau_vel(_params->vel_Tau),
_param_ekf2_tau_pos(_params->pos_Tau),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
Expand All @@ -639,6 +628,22 @@ Ekf2::Ekf2():
updateParams();

_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);

_params->imu_pos_body(0) = _param_ekf2_imu_pos_x.get();
_params->imu_pos_body(1) = _param_ekf2_imu_pos_y.get();
_params->imu_pos_body(2) = _param_ekf2_imu_pos_z.get();
_params->gps_pos_body(0) = _param_ekf2_gps_pos_x.get();
_params->gps_pos_body(1) = _param_ekf2_gps_pos_y.get();
_params->gps_pos_body(2) = _param_ekf2_gps_pos_z.get();
_params->rng_pos_body(0) = _param_ekf2_rng_pos_x.get();
_params->rng_pos_body(1) = _param_ekf2_rng_pos_y.get();
_params->rng_pos_body(2) = _param_ekf2_rng_pos_z.get();
_params->flow_pos_body(0) = _param_ekf2_of_pos_x.get();
_params->flow_pos_body(1) = _param_ekf2_of_pos_y.get();
_params->flow_pos_body(2) = _param_ekf2_of_pos_z.get();
_params->ev_pos_body(0) = _param_ekf2_ev_pos_x.get();
_params->ev_pos_body(1) = _param_ekf2_ev_pos_y.get();
_params->ev_pos_body(2) = _param_ekf2_ev_pos_z.get();
}

Ekf2::~Ekf2()
Expand Down Expand Up @@ -789,12 +794,14 @@ void Ekf2::run()
const hrt_abstime now = sensors.timestamp;

// push imu data into estimator
imuSample imu_sample_new;
estimator::imuSample imu_sample_new;
imu_sample_new.time_us = now;
imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt;
imu_sample_new.delta_ang = matrix::Vector3<ecl_float_t> {sensors.gyro_rad[0], sensors.gyro_rad[1], sensors.gyro_rad[2]} *
imu_sample_new.delta_ang_dt;
imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
imu_sample_new.delta_vel = matrix::Vector3<ecl_float_t> {sensors.accelerometer_m_s2[0], sensors.accelerometer_m_s2[1], sensors.accelerometer_m_s2[2]} *
imu_sample_new.delta_vel_dt;

_ekf.setIMUData(imu_sample_new);

Expand Down Expand Up @@ -1093,7 +1100,7 @@ void Ekf2::run()
optical_flow_s optical_flow;

if (_optical_flow_sub.copy(&optical_flow)) {
flow_message flow;
estimator::flow_message flow;
flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
flow.quality = optical_flow.quality;
Expand Down Expand Up @@ -1159,7 +1166,7 @@ void Ekf2::run()
vehicle_odometry_s ev_odom;
_ev_odom_sub.copy(&ev_odom);

ext_vision_message ev_data;
estimator::ext_vision_message ev_data;

// check for valid position data
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
Expand All @@ -1183,7 +1190,10 @@ void Ekf2::run()

// check for valid orientation data
if (PX4_ISFINITE(ev_odom.q[0])) {
ev_data.quat = matrix::Quatf(ev_odom.q);
ev_data.quat(0) = ev_odom.q[0];
ev_data.quat(1) = ev_odom.q[1];
ev_data.quat(2) = ev_odom.q[2];
ev_data.quat(3) = ev_odom.q[3];

// orientation measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) {
Expand Down Expand Up @@ -1249,7 +1259,7 @@ void Ekf2::run()

if (updated) {

filter_control_status_u control_status;
estimator::filter_control_status_u control_status;
_ekf.get_control_mode(&control_status.value);

// only publish position after successful alignment
Expand Down Expand Up @@ -1404,7 +1414,7 @@ void Ekf2::run()
}

// Get covariances to vehicle odometry
float covariances[24];
ecl_float_t covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);

// get the covariance matrix size
Expand Down Expand Up @@ -1522,7 +1532,14 @@ void Ekf2::run()
status.timestamp = now;
_ekf.get_state_delayed(status.states);
status.n_states = 24;
_ekf.covariances_diagonal().copyTo(status.covariances);

// _ekf.covariances_diagonal().copyTo(status.covariances);
const auto cov = _ekf.covariances_diagonal();

for (int i = 0; i < 24; i++) {
status.covariances[i] = cov(i);
}

_ekf.get_gps_check_status(&status.gps_check_fail_flags);
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
Expand Down Expand Up @@ -2474,7 +2491,7 @@ int Ekf2::task_spawn(int argc, char *argv[])
_task_id = px4_task_spawn_cmd("ekf2",
SCHED_DEFAULT,
SCHED_PRIORITY_ESTIMATOR,
6600,
9600,
(px4_main_t)&run_trampoline,
(char *const *)argv);

Expand Down

0 comments on commit 5dff9e7

Please sign in to comment.