diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 5fe4c16..cb0e271 100644 --- a/rosplane/CMakeLists.txt +++ b/rosplane/CMakeLists.txt @@ -111,8 +111,9 @@ install(TARGETS # Estimator add_executable(rosplane_estimator_node - src/estimator_base.cpp - src/estimator_example.cpp) + src/estimator_ros.cpp + src/estimator_ekf.cpp + src/estimator_continuous_discrete.cpp) target_link_libraries(rosplane_estimator_node ${YAML_CPP_LIBRARIES} ) diff --git a/rosplane/include/estimator_continuous_discrete.hpp b/rosplane/include/estimator_continuous_discrete.hpp new file mode 100644 index 0000000..2bc654f --- /dev/null +++ b/rosplane/include/estimator_continuous_discrete.hpp @@ -0,0 +1,126 @@ +#ifndef ESTIMATOR_CONTINUOUS_DISCRETE_H +#define ESTIMATOR_CONTINUOUS_DISCRETE_H + +#include + +#include +#include + +#include "estimator_ekf.hpp" +#include "estimator_ros.hpp" + +namespace rosplane +{ + +class EstimatorContinuousDiscrete : public EstimatorEKF +{ +public: + EstimatorContinuousDiscrete(); + EstimatorContinuousDiscrete(bool use_params); + +private: + virtual void estimate(const Input & input, Output & output); + + double lpf_a_; + float alpha_; + float alpha1_; + int N_; + + float lpf_gyro_x_; + float lpf_gyro_y_; + float lpf_gyro_z_; + float lpf_static_; + float lpf_diff_; + float lpf_accel_x_; + float lpf_accel_y_; + float lpf_accel_z_; + + float phat_; + float qhat_; + float rhat_; + float Vwhat_; + float phihat_; + float thetahat_; + float psihat_; // TODO: link to an inital condiditons param + + + Eigen::VectorXf attitude_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); + std::function attitude_dynamics_model; + + Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates); + std::function attitude_jacobian_model; + + Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates); + std::function attitude_input_jacobian_model; + + Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); + std::function attitude_measurement_model; + + Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); + std::function attitude_measurement_jacobian_model; + + Eigen::VectorXf position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); + std::function position_dynamics_model; + + Eigen::MatrixXf position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); + std::function position_jacobian_model; + + Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); + std::function position_input_jacobian_model; + + Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input); + std::function position_measurement_model; + + Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input); + std::function position_measurement_jacobian_model; + + Eigen::Vector2f xhat_a_; // 2 + Eigen::Matrix2f P_a_; // 2x2 + + Eigen::VectorXf xhat_p_; // 7 + Eigen::MatrixXf P_p_; // 7x7 + + Eigen::Matrix2f Q_a_; // 2x2 + Eigen::Matrix3f Q_g_; + Eigen::Matrix3f R_accel_; + + Eigen::MatrixXf Q_p_; // 7x7 + Eigen::MatrixXf R_p_; // 6x6 + Eigen::VectorXf f_p_; // 7 + Eigen::MatrixXf A_p_; // 7x7 + float h_p_; + Eigen::VectorXf C_p_; // 7 + Eigen::VectorXf L_p_; // 7 + + // TODO: not used + float gate_threshold_ = 9.21; // chi2(q = .01, df = 2) + + void check_xhat_a(); + + void bind_functions(); + + /** + * @brief This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. + * It also sets the default parameter, which will then be overridden by a launch script. + */ + void declare_parameters(); + + /** + * @brief Initializes some variables that depend on ROS2 parameters + */ + void update_measurement_model_parameters(); + + /** + * @brief Initializes the covariance matrices and process noise matrices with the ROS2 parameters + */ + void initialize_uncertainties(); + + /** + * @brief Initializes the state covariance matrix with the ROS2 parameters + */ + void initialize_state_covariances(); +}; + +} // namespace rosplane + +#endif // ESTIMATOR_CONTINUOUS_DISCRETE_H diff --git a/rosplane/include/estimator_ekf.hpp b/rosplane/include/estimator_ekf.hpp new file mode 100644 index 0000000..4d66ea7 --- /dev/null +++ b/rosplane/include/estimator_ekf.hpp @@ -0,0 +1,53 @@ +#ifndef ESTIMATOR_EKF_H +#define ESTIMATOR_EKF_H + +#include +#include +#include + +#include +#include + +#include "estimator_ros.hpp" + +namespace rosplane +{ + +class EstimatorEKF : public EstimatorROS +{ +public: + EstimatorEKF(); + +protected: + std::tuple measurement_update(Eigen::VectorXf x, + Eigen::VectorXf inputs, + std::function measurement_model, + Eigen::VectorXf y, + std::function measurement_jacobian, + Eigen::MatrixXf R, + Eigen::MatrixXf P); + + std::tuple propagate_model(Eigen::VectorXf x, + std::function dynamic_model, + std::function jacobian, + Eigen::VectorXf inputs, + std::function input_jacobian, + Eigen::MatrixXf P, + Eigen::MatrixXf Q, + Eigen::MatrixXf Q_g, + float Ts); + + std::tuple single_measurement_update(float measurement, + float mesurement_prediction, + float measurement_uncertainty, + Eigen::VectorXf measurement_jacobian, + Eigen::VectorXf x, + Eigen::MatrixXf P); + +private: + virtual void estimate(const Input & input, Output & output) override = 0; +}; + +} // namespace rosplane + +#endif // ESTIMATOR_EKF_H diff --git a/rosplane/include/estimator_example.hpp b/rosplane/include/estimator_example.hpp deleted file mode 100644 index 095362b..0000000 --- a/rosplane/include/estimator_example.hpp +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef ESTIMATOR_EXAMPLE_H -#define ESTIMATOR_EXAMPLE_H - -#include - -#include "estimator_base.hpp" - -namespace rosplane -{ - -class EstimatorExample : public EstimatorBase -{ -public: - EstimatorExample(); - EstimatorExample(bool use_params); - -private: - virtual void estimate(const Input & input, Output & output); - - // float gps_n_old_; - // float gps_e_old_; - // float gps_Vg_old_; - // float gps_course_old_; - - double lpf_a_; - float alpha_; - float alpha1_; - int N_; - - float lpf_gyro_x_; - float lpf_gyro_y_; - float lpf_gyro_z_; - float lpf_static_; - float lpf_diff_; - float lpf_accel_x_; - float lpf_accel_y_; - float lpf_accel_z_; - - float phat_; - float qhat_; - float rhat_; - float Vwhat_; - float phihat_; - float thetahat_; - float psihat_; // TODO link to an inital condiditons param - Eigen::Vector2f xhat_a_; // 2 - Eigen::Matrix2f P_a_; // 2x2 - - Eigen::VectorXf xhat_p_; // 7 - Eigen::MatrixXf P_p_; // 7x7 - - Eigen::Matrix2f Q_a_; // 2x2 - Eigen::Matrix3f Q_g_; - Eigen::Matrix3f R_accel_; - Eigen::Vector2f f_a_; // 2 - Eigen::Matrix2f A_a_; // 2x2 - // float h_a_; - Eigen::Vector3f h_a_; - Eigen::Matrix C_a_; // 2 - Eigen::Vector2f L_a_; // 2 - - Eigen::MatrixXf Q_p_; // 7x7 - Eigen::MatrixXf R_p_; // 6x6 - Eigen::VectorXf f_p_; // 7 - Eigen::MatrixXf A_p_; // 7x7 - float h_p_; - Eigen::VectorXf C_p_; // 7 - Eigen::VectorXf L_p_; // 7 - - // TODO: not used - float gate_threshold_ = 9.21; // chi2(q = .01, df = 2) - - void check_xhat_a(); - - /** - * @brief This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. - * It also sets the default parameter, which will then be overridden by a launch script. - */ - void declare_parameters(); - - /** - * @brief Initializes some variables that depend on ROS2 parameters - */ - void update_measurement_model_parameters(); - - /** - * @brief Initializes the covariance matrices and process noise matrices with the ROS2 parameters - */ - void initialize_uncertainties(); - - /** - * @brief Initializes the state covariance matrix with the ROS2 parameters - */ - void initialize_state_covariances(); -}; - -} // namespace rosplane - -#endif // ESTIMATOR_EXAMPLE_H diff --git a/rosplane/include/estimator_base.hpp b/rosplane/include/estimator_ros.hpp similarity index 90% rename from rosplane/include/estimator_base.hpp rename to rosplane/include/estimator_ros.hpp index acfecc7..cda7c8c 100644 --- a/rosplane/include/estimator_base.hpp +++ b/rosplane/include/estimator_ros.hpp @@ -1,17 +1,19 @@ /** - * @file estimator_base.h + * @file estimator_ros.h * - * Base class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php + * ROS-interface class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php * - * @author Gary Ellingson + * Based on orignal work by Gary Ellingson. + * + * @author Ian Reid */ -#ifndef ESTIMATOR_BASE_H -#define ESTIMATOR_BASE_H +#ifndef ESTIMATOR_ROS_H +#define ESTIMATOR_ROS_H #include -#include +#include #include #include #include @@ -19,6 +21,7 @@ #include #include #include +#include #include "param_manager.hpp" #include "rosplane_msgs/msg/state.hpp" @@ -31,10 +34,10 @@ using namespace std::chrono_literals; namespace rosplane { -class EstimatorBase : public rclcpp::Node +class EstimatorROS : public rclcpp::Node { public: - EstimatorBase(); + EstimatorROS(); protected: struct Input @@ -82,7 +85,6 @@ class EstimatorBase : public rclcpp::Node virtual void estimate(const Input & input, Output & output) = 0; -protected: ParamManager params_; bool gps_init_; double init_lat_ = 0.0; /**< Initial latitude in degrees */ @@ -139,7 +141,7 @@ class EstimatorBase : public rclcpp::Node void declare_parameters(); /** - * @brief Determines the period of a timer based on the ROS2 parameter and starts it + * @brief Determines the period of a timer rosd on the ROS2 parameter and starts it */ void set_timer(); @@ -162,4 +164,4 @@ class EstimatorBase : public rclcpp::Node } // namespace rosplane -#endif // ESTIMATOR_BASE_H +#endif // ESTIMATOR_ROS_H diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_continuous_discrete.cpp similarity index 51% rename from rosplane/src/estimator_example.cpp rename to rosplane/src/estimator_continuous_discrete.cpp index 7849ada..3c964f1 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_continuous_discrete.cpp @@ -1,6 +1,8 @@ -#include "estimator_base.hpp" +#include +#include -#include "estimator_example.hpp" +#include "estimator_continuous_discrete.hpp" +#include "estimator_ros.hpp" namespace rosplane { @@ -13,8 +15,9 @@ double wrap_within_180(double fixed_heading, double wrapped_heading) return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } -EstimatorExample::EstimatorExample() - : xhat_a_(Eigen::Vector2f::Zero()) +EstimatorContinuousDiscrete::EstimatorContinuousDiscrete() + : EstimatorEKF() + , xhat_a_(Eigen::Vector2f::Zero()) , P_a_(Eigen::Matrix2f::Identity()) , xhat_p_(Eigen::VectorXf::Zero(7)) , P_p_(Eigen::MatrixXf::Identity(7, 7)) @@ -28,6 +31,9 @@ EstimatorExample::EstimatorExample() , C_p_(7) , L_p_(7) { + + bind_functions(); // TODO: Document what the _models are. + phat_ = 0; qhat_ = 0; rhat_ = 0; @@ -54,7 +60,7 @@ EstimatorExample::EstimatorExample() N_ = params_.get_int("num_propagation_steps"); } -EstimatorExample::EstimatorExample(bool use_params) : EstimatorExample() +EstimatorContinuousDiscrete::EstimatorContinuousDiscrete(bool use_params) : EstimatorContinuousDiscrete() { double init_lat = params_.get_double("init_lat"); double init_long = params_.get_double("init_lon"); @@ -76,7 +82,7 @@ EstimatorExample::EstimatorExample(bool use_params) : EstimatorExample() init_static_ = init_static; } -void EstimatorExample::initialize_state_covariances() { +void EstimatorContinuousDiscrete::initialize_state_covariances() { double pos_n_initial_cov = params_.get_double("pos_n_initial_cov"); double pos_e_initial_cov = params_.get_double("pos_e_initial_cov"); double vg_initial_cov = params_.get_double("vg_initial_cov"); @@ -95,12 +101,11 @@ void EstimatorExample::initialize_state_covariances() { P_p_(6, 6) = radians(psi_initial_cov); } -void EstimatorExample::initialize_uncertainties() { +void EstimatorContinuousDiscrete::initialize_uncertainties() { double roll_process_noise = params_.get_double("roll_process_noise"); double pitch_process_noise = params_.get_double("pitch_process_noise"); double gyro_process_noise = params_.get_double("gyro_process_noise"); double position_process_noise = params_.get_double("pos_process_noise"); - double attitude_initial_cov = params_.get_double("attitude_initial_cov"); P_a_ *= powf(radians(attitude_initial_cov), 2); @@ -115,7 +120,7 @@ void EstimatorExample::initialize_uncertainties() { initialize_state_covariances(); } -void EstimatorExample::update_measurement_model_parameters() +void EstimatorContinuousDiscrete::update_measurement_model_parameters() { // For readability, declare the parameters used in the function here double sigma_n_gps = params_.get_double("sigma_n_gps"); @@ -145,7 +150,7 @@ void EstimatorExample::update_measurement_model_parameters() alpha1_ = exp(-lpf_a1 * Ts); } -void EstimatorExample::estimate(const Input & input, Output & output) +void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) { // For readability, declare the parameters here double rho = params_.get_double("rho"); @@ -166,6 +171,10 @@ void EstimatorExample::estimate(const Input & input, Output & output) float phat = lpf_gyro_x_; float qhat = lpf_gyro_y_; float rhat = lpf_gyro_z_; + + // These states will allow us to propagate our state model for pitch and roll. + Eigen::Vector3f angular_rates; + angular_rates << phat, qhat, rhat; // low pass filter static pressure sensor and invert to esimate altitude lpf_static_ = alpha1_ * lpf_static_ + (1 - alpha1_) * input.static_pres; @@ -190,206 +199,74 @@ void EstimatorExample::estimate(const Input & input, Output & output) lpf_accel_x_ = alpha_ * lpf_accel_x_ + (1 - alpha_) * input.accel_x; lpf_accel_y_ = alpha_ * lpf_accel_y_ + (1 - alpha_) * input.accel_y; lpf_accel_z_ = alpha_ * lpf_accel_z_ + (1 - alpha_) * input.accel_z; - - // implement continuous-discrete EKF to estimate roll and pitch angles - // prediction step - float cp; // cos(phi) - float sp; // sin(phi) - float tt; // tan(thata) - float ct; // cos(thata)F - float st; // sin(theta) - for (int i = 0; i < N_; i++) { - - cp = cosf(xhat_a_(0)); // cos(phi) - sp = sinf(xhat_a_(0)); // sin(phi) - tt = tanf(xhat_a_(1)); // tan(theta) - ct = cosf(xhat_a_(1)); // cos(theta) - - f_a_(0) = phat + (qhat * sp + rhat * cp) * tt; - f_a_(1) = qhat * cp - rhat * sp; - - xhat_a_ += f_a_ * (Ts / N_); - - cp = cosf(xhat_a_(0)); // cos(phi) - sp = sinf(xhat_a_(0)); // sin(phi) - tt = tanf(xhat_a_(1)); // tan(theta) - ct = cosf(xhat_a_(1)); // cos(theta) - - A_a_ = Eigen::Matrix2f::Zero(); - A_a_(0, 0) = (qhat * cp - rhat * sp) * tt; - A_a_(0, 1) = (qhat * sp + rhat * cp) / ct / ct; - A_a_(1, 0) = -qhat * sp - rhat * cp; - - Eigen::MatrixXf A_d = Eigen::MatrixXf::Identity(2, 2) + Ts / N_ * A_a_ - + pow(Ts / N_, 2) / 2.0 * A_a_ * A_a_; - - Eigen::Matrix G; - G << 1, sp * tt, cp * tt, 0.0, cp, -sp; - - P_a_ = - (A_d * P_a_ * A_d.transpose() + (Q_a_ + G * Q_g_ * G.transpose()) * pow(Ts / N_, 2)); - } - // measurement updates - cp = cosf(xhat_a_(0)); - sp = sinf(xhat_a_(0)); - ct = cosf(xhat_a_(1)); - st = sinf(xhat_a_(1)); - Eigen::Matrix2f I; - I = Eigen::Matrix2f::Identity(); - - h_a_ = Eigen::Vector3f::Zero(3); - h_a_(0) = qhat * vahat * st + gravity * st; - h_a_(1) = rhat * vahat * ct - phat * vahat * st - gravity * ct * sp; - h_a_(2) = -qhat * vahat * ct - gravity * ct * cp; - - C_a_ << 0.0, qhat * vahat * ct + gravity * ct, -gravity * cp * ct, - -rhat * vahat * st - phat * vahat * ct + gravity * sp * st, gravity * sp * ct, - (qhat * vahat + gravity * cp) * st; - - // This calculates the Kalman Gain for all of the attitude states at once rather than one at a time. - - Eigen::Vector3f y; - - y << lpf_accel_x_, lpf_accel_y_, lpf_accel_z_; - - Eigen::MatrixXf S_inv = (R_accel_ + C_a_ * P_a_ * C_a_.transpose()).inverse(); - Eigen::MatrixXf L_a_ = P_a_ * C_a_.transpose() * S_inv; - Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(2, 2) - L_a_ * C_a_; - - P_a_ = temp * P_a_ * temp.transpose() + L_a_ * R_accel_ * L_a_.transpose(); - xhat_a_ = xhat_a_ + L_a_ * (y - h_a_); - + + // These are the current states that will allow us to predict our measurement. + Eigen::Vector4f att_curr_state_info; + att_curr_state_info << angular_rates, vahat; + + // The sensor measurements that we can compare to our preditions. + Eigen::Vector3f y_att; + y_att << lpf_accel_x_, lpf_accel_y_, lpf_accel_z_; + + // ATTITUDE (ROLL AND PITCH) ESTIMATION + // Prediction step + std::tie(P_a_, xhat_a_) = propagate_model(xhat_a_, attitude_dynamics_model, attitude_jacobian_model, angular_rates, + attitude_input_jacobian_model, P_a_, Q_a_, Q_g_, Ts); + + // Measurement update + std::tie(P_a_, xhat_a_) = measurement_update(xhat_a_, att_curr_state_info, attitude_measurement_model, y_att, attitude_measurement_jacobian_model, // TODO: change these std::ties to just pass by reference. + R_accel_,P_a_); + + // Check the estimate for errors check_xhat_a(); + // Store esimate for later use. phihat_ = xhat_a_(0); thetahat_ = xhat_a_(1); - // implement continous-discrete EKF to estimate pn, pe, chi, Vg - // prediction step - float psidot, tmp, Vgdot; + // Implement continous-discrete EKF to estimate pn, pe, chi, Vg, wn, we + // Prediction step + if (fabsf(xhat_p_(2)) < 0.01f) { xhat_p_(2) = 0.01; // prevent divide by zero } - - for (int i = 0; i < N_; i++) { - - float Vg = xhat_p_(2); - float chi = xhat_p_(3); - float wn = xhat_p_(4); - float we = xhat_p_(5); - float psi = xhat_p_(6); - - psidot = (qhat * sinf(phihat_) + rhat * cosf(phihat_)) / cosf(thetahat_); - - tmp = -psidot * vahat * (xhat_p_(4) * cosf(xhat_p_(6)) + xhat_p_(5) * sinf(xhat_p_(6))) - / xhat_p_(2); - Vgdot = vahat / Vg * psidot * (we * cosf(psi) - wn * sinf(psi)); - - f_p_ = Eigen::VectorXf::Zero(7); - f_p_(0) = xhat_p_(2) * cosf(xhat_p_(3)); - f_p_(1) = xhat_p_(2) * sinf(xhat_p_(3)); - f_p_(2) = Vgdot; - f_p_(3) = gravity / xhat_p_(2) * tanf(phihat_) * cosf(chi - psi); - f_p_(6) = psidot; - - xhat_p_ += f_p_ * (Ts / N_); - - A_p_ = Eigen::MatrixXf::Zero(7, 7); - A_p_(0, 2) = cos(xhat_p_(3)); - A_p_(0, 3) = -xhat_p_(2) * sinf(xhat_p_(3)); - A_p_(1, 2) = sin(xhat_p_(3)); - A_p_(1, 3) = xhat_p_(2) * cosf(xhat_p_(3)); - A_p_(2, 2) = -Vgdot / xhat_p_(2); - A_p_(2, 4) = -psidot * vahat * sinf(xhat_p_(6)) / xhat_p_(2); - A_p_(2, 5) = psidot * vahat * cosf(xhat_p_(6)) / xhat_p_(2); - A_p_(2, 6) = tmp; - A_p_(3, 2) = -gravity / powf(xhat_p_(2), 2) * tanf(phihat_); - - Eigen::MatrixXf A_d_ = Eigen::MatrixXf::Identity(7, 7) + Ts / N_ * A_p_ - + A_p_ * A_p_ * pow(Ts / N_, 2) / 2.0; - - P_p_ = (A_d_ * P_p_ * A_d_.transpose() + Q_p_ * pow(Ts / N_, 2)); - } - + + // These are the state that will allow us to propagate our state model for the position state. + Eigen::Vector attitude_states; + attitude_states << angular_rates, xhat_a_(0), xhat_a_(1), vahat; + + // POSITION AND COURSE ESTIMATION + // Prediction step + std::tie(P_p_, xhat_p_) = propagate_model(xhat_p_, position_dynamics_model, position_jacobian_model, attitude_states, position_input_jacobian_model, P_p_, Q_p_, Eigen::MatrixXf::Zero(7, 7), Ts); + + // Check wrapping of the heading and course. xhat_p_(3) = wrap_within_180(0.0, xhat_p_(3)); + xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); if (xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi"); xhat_p_(3) = 0; } - - xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); if (xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi"); xhat_p_(6) = 0; } - // measurement updates - // These calculate the Kalman gain and applies them to each state individually. + // Measurement updates. + // Only update if new GPS information is available. if (input.gps_new) { + Eigen::VectorXf pos_curr_state_info(1); + pos_curr_state_info << vahat; - Eigen::MatrixXf I_p(7, 7); - I_p = Eigen::MatrixXf::Identity(7, 7); - - // gps North position - h_p_ = xhat_p_(0); - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(0) = 1; - L_p_ = (P_p_ * C_p_) / (R_p_(0, 0) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (input.gps_n - h_p_); - - // gps East position - h_p_ = xhat_p_(1); - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(1) = 1; - L_p_ = (P_p_ * C_p_) / (R_p_(1, 1) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (input.gps_e - h_p_); - - // gps ground speed - h_p_ = xhat_p_(2); - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(2) = 1; - L_p_ = (P_p_ * C_p_) / (R_p_(2, 2) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (input.gps_Vg - h_p_); - - // gps course //wrap course measurement float gps_course = fmodf(input.gps_course, radians(360.0f)); - gps_course = wrap_within_180(xhat_p_(3), gps_course); - h_p_ = xhat_p_(3); - - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(3) = 1; - L_p_ = (P_p_ * C_p_) / (R_p_(3, 3) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (gps_course - h_p_); - - // pseudo measurement #1 y_1 = va*cos(psi)+wn-Vg*cos(chi) - h_p_ = - vahat * cosf(xhat_p_(6)) + xhat_p_(4) - xhat_p_(2) * cosf(xhat_p_(3)); // pseudo measurement - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(2) = -cos(xhat_p_(3)); - C_p_(3) = xhat_p_(2) * sinf(xhat_p_(3)); - C_p_(4) = 1; - C_p_(6) = -vahat * sinf(xhat_p_(6)); - L_p_ = (P_p_ * C_p_) / (R_p_(4, 4) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (0 - h_p_); - - // pseudo measurement #2 y_2 = va*sin(psi) + we - Vg*sin(chi) - h_p_ = - vahat * sinf(xhat_p_(6)) + xhat_p_(5) - xhat_p_(2) * sinf(xhat_p_(3)); // pseudo measurement - C_p_ = Eigen::VectorXf::Zero(7); - C_p_(2) = -sin(xhat_p_(3)); - C_p_(3) = -xhat_p_(2) * cosf(xhat_p_(3)); - C_p_(5) = 1; - C_p_(6) = vahat * cosf(xhat_p_(6)); - L_p_ = (P_p_ * C_p_) / (R_p_(5, 5) + (C_p_.transpose() * P_p_ * C_p_)); - P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; - xhat_p_ = xhat_p_ + L_p_ * (0 - h_p_); + + // Measurements for the postional states. + Eigen::Vector y_pos; + y_pos << input.gps_n, input.gps_e, input.gps_Vg, gps_course, 0.0, 0.0; + + // Update the state and covariance with based on the predicted and actual measurements. + std::tie(P_p_, xhat_p_) = measurement_update(xhat_p_, pos_curr_state_info, position_measurement_model, y_pos, position_measurement_jacobian_model, R_p_, P_p_); if (xhat_p_(0) > gps_n_lim || xhat_p_(0) < -gps_n_lim) { RCLCPP_WARN(this->get_logger(), "gps n limit reached"); @@ -437,7 +314,6 @@ void EstimatorExample::estimate(const Input & input, Output & output) prob_index); } if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) { - //xhat_p(3) = fmodf(xhat_p(3),2.0*M_PI); xhat_p_(6) = fmodf(xhat_p_(6), 2.0 * M_PI); } @@ -467,7 +343,244 @@ void EstimatorExample::estimate(const Input & input, Output & output) output.psi = psihat; } -void EstimatorExample::check_xhat_a() +Eigen::VectorXf EstimatorContinuousDiscrete::attitude_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates) +{ + float cp = cosf(state(0)); // cos(phi) + float sp = sinf(state(0)); // sin(phi) + float tt = tanf(state(1)); // tan(theta) + + float p = angular_rates(0); + float q = angular_rates(1); + float r = angular_rates(2); + + Eigen::Vector2f f; + + f(0) = p + (q * sp + r * cp) * tt; + f(1) = q * cp - r * sp; + + return f; +} + +Eigen::VectorXf EstimatorContinuousDiscrete::position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements) +{ + + double gravity = params_.get_double("gravity"); + + float Vg = state(2); + float chi = state(3); + float wn = state(4); + float we = state(5); + float psi = state(6); + + float p = measurements(0); + float q = measurements(1); + float r = measurements(2); + float phi = measurements(3); + float theta = measurements(4); + float va = measurements(5); + + float psidot = (q * sinf(phi) + r * cosf(phi)) / cosf(theta); + + float Vgdot = va / Vg * psidot * (we * cosf(psi) - wn * sinf(psi)); + + Eigen::VectorXf f; + f = Eigen::VectorXf::Zero(7); + + f(0) = state(2) * cosf(state(3)); + f(1) = state(2) * sinf(state(3)); + f(2) = Vgdot; + f(3) = gravity / state(2) * tanf(phi) * cosf(chi - psi); + f(6) = psidot; + + return f; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates) +{ + float cp = cosf(state(0)); // cos(phi) + float sp = sinf(state(0)); // sin(phi) + float tt = tanf(state(1)); // tan(theta) + float ct = cosf(state(1)); // cos(theta) + + float q = angular_rates(1); + float r = angular_rates(2); + + Eigen::Matrix2f A = Eigen::Matrix2f::Zero(); + A(0, 0) = (q * cp - r * sp) * tt; + A(0, 1) = (q * sp + r * cp) / ct / ct; + A(1, 0) = -q * sp - r * cp; + + return A; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements) +{ + double gravity = params_.get_double("gravity"); + + float p = measurements(0); + float q = measurements(1); + float r = measurements(2); + float phi = measurements(3); + float theta = measurements(4); + float va = measurements(5); + + float Vg = state(2); + float chi = state(3); + float wn = state(4); + float we = state(5); + float psi = state(6); + + float psidot = (q * sinf(phi) + r * cosf(phi)) / cosf(theta); + + float tmp = -psidot * va * (state(4) * cosf(state(6)) + state(5) * sinf(state(6))) / state(2); + + float Vgdot = va / Vg * psidot * (wn * cosf(psi) - we * sinf(psi)); + + Eigen::MatrixXf A; + A = Eigen::MatrixXf::Zero(7, 7); + A(0, 2) = cos(state(3)); + A(0, 3) = -state(2) * sinf(state(3)); + A(1, 2) = sin(state(3)); + A(1, 3) = state(2) * cosf(state(3)); + A(2, 2) = -Vgdot / state(2); + A(2, 4) = -psidot * va * sinf(state(6)) / state(2); + A(2, 5) = psidot * va * cosf(state(6)) / state(2); + A(2, 6) = tmp; + A(3, 2) = -gravity / powf(state(2), 2) * tanf(phihat_); + + return A; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +{ + float cp = cosf(state(0)); // cos(phi) + float sp = sinf(state(0)); // sin(phi) + float tt = tanf(state(1)); // tan(theta) + + Eigen::Matrix G; + G << 1, sp * tt, cp * tt, 0.0, cp, -sp; + + return G; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +{ + + Eigen::MatrixXf G; + G = Eigen::MatrixXf::Zero(7, 7); + + return G; +} + +Eigen::VectorXf EstimatorContinuousDiscrete::attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +{ + double gravity = params_.get_double("gravity"); + float cp = cosf(state(0)); // cos(phi) + float sp = sinf(state(0)); // sin(phi) + float st = sinf(state(1)); // sin(theta) + float ct = cosf(state(1)); // cos(theta) + + float p = inputs(0); + float q = inputs(1); + float r = inputs(2); + float va = inputs(3); + + Eigen::Vector3f h; + h = Eigen::Vector3f::Zero(3); + h(0) = q * va * st + gravity * st; + h(1) = r * va * ct - p * va * st - gravity * ct * sp; + h(2) = -q * va * ct - gravity * ct * cp; + + return h; +} + +Eigen::VectorXf EstimatorContinuousDiscrete::position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input) +{ + float va = input(0); + + Eigen::VectorXf h = Eigen::VectorXf::Zero(6); + + // GPS north + h(0) = state(0); + + // GPS east + h(1) = state(1); + + // GPS ground speed + h(2) = state(2); + + // GPS course + h(3) = state(3); + + // Pseudo Measurement north + h(4) = va * cosf(state(6)) + state(4) - state(2) * cosf(state(3)); + + // Pseudo Measurement east + h(5) = va * sinf(state(6)) + state(5) - state(2) * sinf(state(3)); + + // To add a new measurement, simply use the state and any input you need as another entry to h. Be sure to update the measurement jacobian C. + + return h; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input) +{ + float va = input(0); + + Eigen::MatrixXf C = Eigen::MatrixXf::Zero(6,7); + + // GPS north + C(0,0) = 1; + + // GPS east + C(1,1) = 1; + + // GPS ground speed + C(2,2) = 1; + + // GPS course + C(3,3) = 1; + + // Pseudo Measurement north + C(4,2) = -cos(state(3)); + C(4,3) = state(2) * sinf(state(3)); + C(4,4) = 1; + C(4,6) = -va * sinf(state(6)); + + // Pseudo Measurement east + C(5, 2) = -sin(state(3)); + C(5, 3) = -state(2) * cosf(state(3)); + C(5, 5) = 1; + C(5, 6) = va * cosf(state(6)); + + // To add a new measurement use the inputs and the state to add another row to the matrix C. Be sure to update the measurment prediction vector h. + + return C; +} + +Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +{ + double gravity = params_.get_double("gravity"); + float cp = cosf(state(0)); + float sp = sinf(state(0)); + float ct = cosf(state(1)); + float st = sinf(state(1)); + + float p = inputs(0); + float q = inputs(1); + float r = inputs(2); + float va = inputs(3); + + Eigen::Matrix C; + + C << 0.0, q * va * ct + gravity * ct, -gravity * cp * ct, + -r * va * st - p * va * ct + gravity * sp * st, gravity * sp * ct, + (q * va + gravity * cp) * st; + + return C; +} + +void EstimatorContinuousDiscrete::check_xhat_a() { double max_phi = params_.get_double("max_estimated_phi"); double max_theta = params_.get_double("max_estimated_theta"); @@ -503,7 +616,7 @@ void EstimatorExample::check_xhat_a() } } -void EstimatorExample::declare_parameters() +void EstimatorContinuousDiscrete::declare_parameters() { params_.declare_double("sigma_n_gps", .01); params_.declare_double("sigma_e_gps", .01); @@ -539,4 +652,22 @@ void EstimatorExample::declare_parameters() params_.declare_double("estimator_max_buffer", 3.0); // Deg } +void EstimatorContinuousDiscrete::bind_functions() +{ + // This creates references to the functions that are necessary estimate. This means we can pass them to the EKF class's functions. + // std::bind creates a forwarding reference to a function. So when we pass the binding object to another method, that method can call the + // original function. + attitude_dynamics_model = std::bind(&EstimatorContinuousDiscrete::attitude_dynamics, this, std::placeholders::_1, std::placeholders::_2); + attitude_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_jacobian, this, std::placeholders::_1, std::placeholders::_2); + attitude_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_input_jacobian, this, std::placeholders::_1, std::placeholders::_2); + attitude_measurement_model = std::bind(&EstimatorContinuousDiscrete::attitude_measurement_prediction, this, std::placeholders::_1, std::placeholders::_2); + attitude_measurement_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_measurement_jacobian, this, std::placeholders::_1, std::placeholders::_2); + position_dynamics_model = std::bind(&EstimatorContinuousDiscrete::position_dynamics, this, std::placeholders::_1, std::placeholders::_2); + position_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_jacobian, this, std::placeholders::_1, std::placeholders::_2); + position_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_input_jacobian, this, std::placeholders::_1, std::placeholders::_2); + position_measurement_model = std::bind(&EstimatorContinuousDiscrete::position_measurement_prediction, this, std::placeholders::_1, std::placeholders::_2); + position_measurement_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_measurement_jacobian, this, std::placeholders::_1, std::placeholders::_2); + +} + } // namespace rosplane diff --git a/rosplane/src/estimator_ekf.cpp b/rosplane/src/estimator_ekf.cpp new file mode 100644 index 0000000..3938867 --- /dev/null +++ b/rosplane/src/estimator_ekf.cpp @@ -0,0 +1,94 @@ +#include +#include + +#include + +#include "estimator_ekf.hpp" +#include "estimator_ros.hpp" + +namespace rosplane +{ + +EstimatorEKF::EstimatorEKF() : EstimatorROS() +{} + +std::tuple EstimatorEKF::measurement_update(Eigen::VectorXf x, + Eigen::VectorXf inputs, + std::function measurement_model, + Eigen::VectorXf y, + std::function measurement_jacobian, + Eigen::MatrixXf R, + Eigen::MatrixXf P) +{ + + Eigen::VectorXf h = measurement_model(x, inputs); + Eigen::MatrixXf C = measurement_jacobian(x, inputs); + + // Find the S_inv to find the Kalman gain. + Eigen::MatrixXf S_inv = (R + C * P * C.transpose()).inverse(); + // Find the Kalman gain. + Eigen::MatrixXf L = P * C.transpose() * S_inv; + // Use a temp to increase readablility. + Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(x.size(), x.size()) - L * C; + + // Adjust the covariance with new information. + P = temp * P * temp.transpose() + L * R * L.transpose(); + // Use Kalman gain to optimally adjust estimate. + x = x + L * (y - h); + + std::tuple result(P, x); + + return result; +} + +std::tuple EstimatorEKF::propagate_model(Eigen::VectorXf x, + std::function dynamic_model, + std::function jacobian, + Eigen::VectorXf inputs, + std::function input_jacobian, + Eigen::MatrixXf P, + Eigen::MatrixXf Q, + Eigen::MatrixXf Q_g, + float Ts) +{ + + int N = params_.get_int("num_propagation_steps"); // TODO: Declare this parameter in the ekf class. + + for (int _ = 0; _ < N; _++) + { + + Eigen::VectorXf f = dynamic_model(x, inputs); + // Propagate model by a step. + x += f * (Ts/N); + + Eigen::MatrixXf A = jacobian(x, inputs); + + // Find the second order approx of the matrix exponential. + Eigen::MatrixXf A_d = Eigen::MatrixXf::Identity(A.rows(), A.cols()) + Ts / N * A + + pow(Ts / N, 2) / 2.0 * A * A; + + 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); + + } + + std::tuple result(P, x); + + return result; +} + +std::tuple EstimatorEKF::single_measurement_update(float measurement, float measurement_prediction, float measurement_variance, Eigen::VectorXf measurement_jacobian, Eigen::VectorXf x, Eigen::MatrixXf P) +{ + Eigen::MatrixXf I(7,7); + I = Eigen::MatrixXf::Identity(x.size(), x.size()); + Eigen::VectorXf L = (P * measurement_jacobian) / (measurement_variance + (measurement_jacobian.transpose() * P * measurement_jacobian)); + P = (I - L * measurement_jacobian.transpose()) * P; + x = x + L * (measurement - measurement_prediction); + + std::tuple result(P,x); + return result; +} + +} // end nampspace. diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_ros.cpp similarity index 84% rename from rosplane/src/estimator_base.cpp rename to rosplane/src/estimator_ros.cpp index 1f653ab..1234a13 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_ros.cpp @@ -2,35 +2,30 @@ #include #include #include -#include -#include -#include - -#include "estimator_example.hpp" - -#include "estimator_base.hpp" +#include "estimator_ros.hpp" +#include "estimator_continuous_discrete.hpp" namespace rosplane { -EstimatorBase::EstimatorBase() - : Node("estimator_base"), params_(this), params_initialized_(false) +EstimatorROS::EstimatorROS() + : Node("estimator_ros"), params_(this), params_initialized_(false) { vehicle_state_pub_ = this->create_publisher("estimated_state", 10); gnss_fix_sub_ = this->create_subscription( - gnss_fix_topic_, 10, std::bind(&EstimatorBase::gnssFixCallback, this, std::placeholders::_1)); + gnss_fix_topic_, 10, std::bind(&EstimatorROS::gnssFixCallback, this, std::placeholders::_1)); gnss_vel_sub_ = this->create_subscription( - gnss_vel_topic_, 10, std::bind(&EstimatorBase::gnssVelCallback, this, std::placeholders::_1)); + gnss_vel_topic_, 10, std::bind(&EstimatorROS::gnssVelCallback, this, std::placeholders::_1)); imu_sub_ = this->create_subscription( - imu_topic_, 10, std::bind(&EstimatorBase::imuCallback, this, std::placeholders::_1)); + imu_topic_, 10, std::bind(&EstimatorROS::imuCallback, this, std::placeholders::_1)); baro_sub_ = this->create_subscription( - baro_topic_, 10, std::bind(&EstimatorBase::baroAltCallback, this, std::placeholders::_1)); + baro_topic_, 10, std::bind(&EstimatorROS::baroAltCallback, this, std::placeholders::_1)); airspeed_sub_ = this->create_subscription( - airspeed_topic_, 10, std::bind(&EstimatorBase::airspeedCallback, this, std::placeholders::_1)); + airspeed_topic_, 10, std::bind(&EstimatorROS::airspeedCallback, this, std::placeholders::_1)); status_sub_ = this->create_subscription( - status_topic_, 10, std::bind(&EstimatorBase::statusCallback, this, std::placeholders::_1)); + status_topic_, 10, std::bind(&EstimatorROS::statusCallback, this, std::placeholders::_1)); init_static_ = 0; baro_count_ = 0; @@ -40,7 +35,7 @@ EstimatorBase::EstimatorBase() // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&EstimatorBase::parametersCallback, this, std::placeholders::_1)); + std::bind(&EstimatorROS::parametersCallback, this, std::placeholders::_1)); // Declare and set parameters with the ROS2 system declare_parameters(); @@ -59,7 +54,7 @@ EstimatorBase::EstimatorBase() set_timer(); } -void EstimatorBase::declare_parameters() +void EstimatorROS::declare_parameters() { params_.declare_double("estimator_update_frequency", 100.0); params_.declare_double("rho", 1.225); @@ -74,15 +69,15 @@ void EstimatorBase::declare_parameters() params_.declare_double("init_alt", 0.0); } -void EstimatorBase::set_timer() { +void EstimatorROS::set_timer() { double frequency = params_.get_double("estimator_update_frequency"); update_period_ = std::chrono::microseconds(static_cast(1.0 / frequency * 1'000'000)); - update_timer_ = this->create_wall_timer(update_period_, std::bind(&EstimatorBase::update, this)); + update_timer_ = this->create_wall_timer(update_period_, std::bind(&EstimatorROS::update, this)); } rcl_interfaces::msg::SetParametersResult -EstimatorBase::parametersCallback(const std::vector & parameters) +EstimatorROS::parametersCallback(const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -109,7 +104,7 @@ EstimatorBase::parametersCallback(const std::vector & paramet return result; } -void EstimatorBase::update() +void EstimatorROS::update() { Output output; @@ -177,7 +172,7 @@ void EstimatorBase::update() vehicle_state_pub_->publish(msg); } -void EstimatorBase::gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +void EstimatorROS::gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { bool has_fix = msg->status.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes @@ -202,7 +197,7 @@ void EstimatorBase::gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr } } -void EstimatorBase::gnssVelCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) +void EstimatorROS::gnssVelCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) { // Rename parameter here for clarity double ground_speed_threshold = params_.get_double("gps_ground_speed_threshold"); @@ -218,7 +213,7 @@ void EstimatorBase::gnssVelCallback(const geometry_msgs::msg::TwistStamped::Shar input_.gps_course = course; } -void EstimatorBase::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) +void EstimatorROS::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { input_.accel_x = msg->linear_acceleration.x; input_.accel_y = msg->linear_acceleration.y; @@ -229,7 +224,7 @@ void EstimatorBase::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) input_.gyro_z = msg->angular_velocity.z; } -void EstimatorBase::baroAltCallback(const rosflight_msgs::msg::Barometer::SharedPtr msg) +void EstimatorROS::baroAltCallback(const rosflight_msgs::msg::Barometer::SharedPtr msg) { // For readability, declare the parameters here double rho = params_.get_double("rho"); @@ -285,7 +280,7 @@ void EstimatorBase::baroAltCallback(const rosflight_msgs::msg::Barometer::Shared } } -void EstimatorBase::airspeedCallback(const rosflight_msgs::msg::Airspeed::SharedPtr msg) +void EstimatorROS::airspeedCallback(const rosflight_msgs::msg::Airspeed::SharedPtr msg) { // For readability, declare the parameters here double rho = params_.get_double("rho"); @@ -302,12 +297,12 @@ void EstimatorBase::airspeedCallback(const rosflight_msgs::msg::Airspeed::Shared } } -void EstimatorBase::statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg) +void EstimatorROS::statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg) { if (!armed_first_time_ && msg->armed) armed_first_time_ = true; } -void EstimatorBase::saveParameter(std::string param_name, double param_val) +void EstimatorROS::saveParameter(std::string param_name, double param_val) { YAML::Node param_yaml_file = YAML::LoadFile(param_filepath_); @@ -332,21 +327,25 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - char* use_params = argv[1]; + char* use_params; // HACK: Fix in a more permanant way. + if (argc >= 1) { + use_params = argv[1]; + } + if (!strcmp(use_params, "true")) { - rclcpp::spin(std::make_shared(use_params)); + rclcpp::spin(std::make_shared(use_params)); } else if(strcmp(use_params, "false")) // If the string is not true or false print error. { - auto estimator_node = std::make_shared(); + auto estimator_node = std::make_shared(); RCLCPP_WARN(estimator_node->get_logger(), "Invalid option for seeding estimator, defaulting to unseeded."); rclcpp::spin(estimator_node); } else { - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); }