From ae85dec68b16995bc707aea9f50b662c7c3b0da1 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 18 May 2026 10:42:22 -0600 Subject: [PATCH] fix: corrected continuous discrete update --- .../src/ekf/estimator_continuous_discrete.cpp | 36 +++++++++---------- roscopter/src/ekf/estimator_ekf.cpp | 6 ++-- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/roscopter/src/ekf/estimator_continuous_discrete.cpp b/roscopter/src/ekf/estimator_continuous_discrete.cpp index c2cd011..fd02024 100644 --- a/roscopter/src/ekf/estimator_continuous_discrete.cpp +++ b/roscopter/src/ekf/estimator_continuous_discrete.cpp @@ -800,31 +800,31 @@ void EstimatorContinuousDiscrete::declare_parameters() { // Sensor uncertainties - params_.declare_double("sigma_n_gps", .5); - params_.declare_double("sigma_e_gps", .5); - params_.declare_double("sigma_h_gps", 1.0); - params_.declare_double("sigma_vn_gps", .07); - params_.declare_double("sigma_ve_gps", .07); - params_.declare_double("sigma_vd_gps", .1); - params_.declare_double("sigma_static_press", 0.5); + params_.declare_double("sigma_n_gps", .75); + params_.declare_double("sigma_e_gps", .75); + params_.declare_double("sigma_h_gps", 1.5); + params_.declare_double("sigma_vn_gps", .02); + params_.declare_double("sigma_ve_gps", .02); + params_.declare_double("sigma_vd_gps", .01); + params_.declare_double("sigma_static_press", 2.0); params_.declare_double("sigma_mag", 0.004); - params_.declare_double("sigma_tilt_mag", radians(0.02)); - params_.declare_double("sigma_accel", .025 * 9.81); + params_.declare_double("sigma_tilt_mag", radians(1.0)); // TODO: Check this!!!! + params_.declare_double("sigma_accel", .5); // Low pass filter parameters params_.declare_double("gyro_cutoff_freq", 20.0); params_.declare_double("baro_cutoff_freq", 1.25); // Proccess noises - params_.declare_double("roll_process_noise", 1000*powf(0.0001,2)); - params_.declare_double("pitch_process_noise", 1000*powf(0.0001,2)); - params_.declare_double("yaw_process_noise", 1000*powf(0.0001,2)); - params_.declare_double("gyro_process_noise", 0.13); - params_.declare_double("accel_process_noise", 0.24525); - params_.declare_double("pos_process_noise", 1000*powf(0.00003,2)); - params_.declare_double("alt_process_noise", 1000*0.000001); - params_.declare_double("vel_horizontal_process_noise", 1000*powf(0.0001,2)); - params_.declare_double("vel_vertical_process_noise", 1000*powf(0.0001,2)); + params_.declare_double("roll_process_noise", powf(0.0001,2)); + params_.declare_double("pitch_process_noise", powf(0.0001,2)); + params_.declare_double("yaw_process_noise", powf(0.001,2)); + params_.declare_double("gyro_process_noise", 0.16); + params_.declare_double("accel_process_noise", 0.6); + params_.declare_double("pos_process_noise", powf(0.00003,2)); + params_.declare_double("alt_process_noise", 0.000001); + params_.declare_double("vel_horizontal_process_noise", powf(0.0001,2)); + params_.declare_double("vel_vertical_process_noise", powf(0.001,2)); params_.declare_double("bias_process_noise", 0.0000001*0.0000001); // Initial covariances diff --git a/roscopter/src/ekf/estimator_ekf.cpp b/roscopter/src/ekf/estimator_ekf.cpp index 767e338..4525554 100644 --- a/roscopter/src/ekf/estimator_ekf.cpp +++ b/roscopter/src/ekf/estimator_ekf.cpp @@ -10,7 +10,7 @@ namespace roscopter EstimatorEKF::EstimatorEKF() : EstimatorROS() { - params_.declare_int("num_propagation_steps", 1); + params_.declare_int("num_propagation_steps", 10); params_.set_parameters(); } @@ -72,7 +72,7 @@ std::tuple EstimatorEKF::propagate_model(Eigen { int N = params_.get_int("num_propagation_steps"); - + float Ts_imu = Ts; // TODO: make this correct. for (int _ = 0; _ < N; _++) { @@ -89,7 +89,7 @@ std::tuple EstimatorEKF::propagate_model(Eigen Eigen::MatrixXf G = input_jacobian(x, inputs); // Propagate the covariance. - P = A_d * P * A_d.transpose() + (Q + G * Q_g * G.transpose()) * pow(Ts / N, 2); + P = A_d * P * A_d.transpose() + Q * Ts / N + G * Q_g * Ts_imu * G.transpose() * Ts / N; }