31 estimator ekf refactor

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}
 )

rosplane/include/estimator_continuous_discrete.hpp

@@ -0,0 +1,126 @@
+#ifndef ESTIMATOR_CONTINUOUS_DISCRETE_H
+#define ESTIMATOR_CONTINUOUS_DISCRETE_H
+
+#include <math.h>
+
+#include <Eigen/Geometry>
+#include <yaml-cpp/yaml.h>
+
+#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<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_dynamics_model;
+
+  Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> attitude_jacobian_model;
+
+  Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> attitude_input_jacobian_model;
+
+  Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_measurement_model;
+
+  Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_measurement_jacobian_model;
+
+  Eigen::VectorXf position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> position_dynamics_model;
+
+  Eigen::MatrixXf position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> position_jacobian_model;
+
+  Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> position_input_jacobian_model;
+
+  Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> position_measurement_model;
+
+  Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> 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

rosplane/include/estimator_ekf.hpp

@@ -0,0 +1,53 @@
+#ifndef ESTIMATOR_EKF_H
+#define ESTIMATOR_EKF_H
+
+#include <cassert>
+#include <math.h>
+#include <tuple>
+
+#include <Eigen/Geometry>
+#include <yaml-cpp/yaml.h>
+
+#include "estimator_ros.hpp"
+
+namespace rosplane
+{
+
+class EstimatorEKF : public EstimatorROS
+{
+public:
+  EstimatorEKF();
+
+protected:
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> measurement_update(Eigen::VectorXf x,
+                                                                  Eigen::VectorXf inputs,
+                                                                  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_model,
+                                                                  Eigen::VectorXf y,
+                                                                  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_jacobian,
+                                                                  Eigen::MatrixXf R,
+                                                                  Eigen::MatrixXf P);
+
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> propagate_model(Eigen::VectorXf x,
+                                                               std::function<Eigen::VectorXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> dynamic_model,
+                                                               std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> jacobian,
+                                                               Eigen::VectorXf inputs,
+                                                               std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> input_jacobian,
+                                                               Eigen::MatrixXf P,
+                                                               Eigen::MatrixXf Q,
+                                                               Eigen::MatrixXf Q_g,
+                                                               float Ts);          
+
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> 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

rosplane/include/estimator_base.hpp → rosplane/include/estimator_ros.hpp

@@ -1,24 +1,27 @@
 /**
- * @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 <gary.ellingson@byu.edu>
+ * Based on orignal work by Gary Ellingson.
+ *
+ * @author Ian Reid <ian.reid@byu.edu>
  */
 
-#ifndef ESTIMATOR_BASE_H
-#define ESTIMATOR_BASE_H
+#ifndef ESTIMATOR_ROS_H
+#define ESTIMATOR_ROS_H
 
 #include <chrono>
-#include <yaml-cpp/yaml.h>
 
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/airspeed.hpp>
 #include <rosflight_msgs/msg/barometer.hpp>
 #include <rosflight_msgs/msg/status.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <yaml-cpp/yaml.h>
 
 #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