Add setting to Kalman filter

tony-hj · Feb 28, 2023 · 0c5ded4 · 0c5ded4
1 parent 7b58e74
commit 0c5ded4
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 19 deletions.
diff --git a/legged_controllers/config/a1/task.info b/legged_controllers/config/a1/task.info
@@ -305,3 +305,15 @@ weight
     baseAccel       1
     contactForce    0.01
 }
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}
diff --git a/legged_controllers/config/aliengo/task.info b/legged_controllers/config/aliengo/task.info
@@ -305,3 +305,15 @@ weight
     baseAccel       1
     contactForce    0.01
 }
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}
diff --git a/legged_controllers/include/legged_controllers/LeggedController.h b/legged_controllers/include/legged_controllers/LeggedController.h
@@ -41,7 +41,7 @@ class LeggedController : public controller_interface::MultiInterfaceController<H
                                     bool verbose);
   virtual void setupMpc();
   virtual void setupMrt();
-  virtual void setupStateEstimate(const std::string& taskFile);
+  virtual void setupStateEstimate(const std::string& taskFile, bool verbose);
 
   // Interface
   std::shared_ptr<LeggedInterface> leggedInterface_;
@@ -78,7 +78,7 @@ class LeggedController : public controller_interface::MultiInterfaceController<H
 
 class LeggedCheaterController : public LeggedController {
  protected:
-  void setupStateEstimate(const std::string& taskFile) override;
+  void setupStateEstimate(const std::string& taskFile, bool verbose) override;
 };
 
 }  // namespace legged
diff --git a/legged_controllers/src/LeggedController.cpp b/legged_controllers/src/LeggedController.cpp
@@ -63,7 +63,7 @@ bool LeggedController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand
   imuSensorHandle_ = robot_hw->get<hardware_interface::ImuSensorInterface>()->getHandle("unitree_imu");
 
   // State estimation
-  setupStateEstimate(taskFile);
+  setupStateEstimate(taskFile, verbose);
 
   // Whole body control
   wbc_ = std::make_shared<WeightedWbc>(leggedInterface_->getPinocchioInterface(), leggedInterface_->getCentroidalModelInfo(),
@@ -250,13 +250,14 @@ void LeggedController::setupMrt() {
   setThreadPriority(leggedInterface_->sqpSettings().threadPriority, mpcThread_);
 }
 
-void LeggedController::setupStateEstimate(const std::string& /*taskFile*/) {
+void LeggedController::setupStateEstimate(const std::string& taskFile, bool verbose) {
   stateEstimate_ = std::make_shared<KalmanFilterEstimate>(leggedInterface_->getPinocchioInterface(),
                                                           leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_);
+  dynamic_cast<KalmanFilterEstimate&>(*stateEstimate_).loadSettings(taskFile, verbose);
   currentObservation_.time = 0;
 }
 
-void LeggedCheaterController::setupStateEstimate(const std::string& /*taskFile*/) {
+void LeggedCheaterController::setupStateEstimate(const std::string& /*taskFile*/, bool /*verbose*/) {
   stateEstimate_ = std::make_shared<FromTopicStateEstimate>(leggedInterface_->getPinocchioInterface(),
                                                             leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_);
 }

diff --git a/legged_estimation/include/legged_estimation/LinearKalmanFilter.h b/legged_estimation/include/legged_estimation/LinearKalmanFilter.h
@@ -23,6 +23,8 @@ class KalmanFilterEstimate : public StateEstimateBase {
 
   vector_t update(const ros::Time& time, const ros::Duration& period) override;
 
+  void loadSettings(const std::string& taskFile, bool verbose);
+
  private:
   void updateFromTopic();
 
@@ -40,8 +42,15 @@ class KalmanFilterEstimate : public StateEstimateBase {
   Eigen::Matrix<scalar_t, 18, 3> b_;
   Eigen::Matrix<scalar_t, 28, 18> c_;
 
-  const scalar_t footRadius_ = 0.02;
+  // Config
   vector_t feetHeights_;
+  scalar_t footRadius_ = 0.02;
+  scalar_t imuProcessNoisePosition_ = 0.02;
+  scalar_t imuProcessNoiseVelocity_ = 0.02;
+  scalar_t footProcessNoisePosition_ = 0.002;
+  scalar_t footSensorNoisePosition_ = 0.005;
+  scalar_t footSensorNoiseVelocity_ = 0.1;
+  scalar_t footHeightSensorNoise_ = 0.01;
 
   // Topic
   ros::Subscriber sub_;

diff --git a/legged_estimation/src/LinearKalmanFilter.cpp b/legged_estimation/src/LinearKalmanFilter.cpp
@@ -87,22 +87,15 @@ vector_t KalmanFilterEstimate::update(const ros::Time& time, const ros::Duration
   const auto eePos = eeKinematics_->getPosition(vector_t());
   const auto eeVel = eeKinematics_->getVelocity(vector_t(), vector_t());
 
-  scalar_t imuProcessNoisePosition = 0.02;
-  scalar_t imuProcessNoiseVelocity = 0.02;
-  scalar_t footProcessNoisePosition = 0.002;
-  scalar_t footSensorNoisePosition = 0.005;
-  scalar_t footSensorNoiseVelocity = 0.1;  // TODO(qiayuan): adjust the value
-  scalar_t footHeightSensorNoise = 0.01;
-
   Eigen::Matrix<scalar_t, 18, 18> q = Eigen::Matrix<scalar_t, 18, 18>::Identity();
-  q.block(0, 0, 3, 3) = q_.block(0, 0, 3, 3) * imuProcessNoisePosition;
-  q.block(3, 3, 3, 3) = q_.block(3, 3, 3, 3) * imuProcessNoiseVelocity;
-  q.block(6, 6, 12, 12) = q_.block(6, 6, 12, 12) * footProcessNoisePosition;
+  q.block(0, 0, 3, 3) = q_.block(0, 0, 3, 3) * imuProcessNoisePosition_;
+  q.block(3, 3, 3, 3) = q_.block(3, 3, 3, 3) * imuProcessNoiseVelocity_;
+  q.block(6, 6, 12, 12) = q_.block(6, 6, 12, 12) * footProcessNoisePosition_;
 
   Eigen::Matrix<scalar_t, 28, 28> r = Eigen::Matrix<scalar_t, 28, 28>::Identity();
-  r.block(0, 0, 12, 12) = r_.block(0, 0, 12, 12) * footSensorNoisePosition;
-  r.block(12, 12, 12, 12) = r_.block(12, 12, 12, 12) * footSensorNoiseVelocity;
-  r.block(24, 24, 4, 4) = r_.block(24, 24, 4, 4) * footHeightSensorNoise;
+  r.block(0, 0, 12, 12) = r_.block(0, 0, 12, 12) * footSensorNoisePosition_;
+  r.block(12, 12, 12, 12) = r_.block(12, 12, 12, 12) * footSensorNoiseVelocity_;
+  r.block(24, 24, 4, 4) = r_.block(24, 24, 4, 4) * footHeightSensorNoise_;
 
   for (int i = 0; i < 4; i++) {
     int i1 = 3 * i;
@@ -262,4 +255,22 @@ nav_msgs::Odometry KalmanFilterEstimate::getOdomMsg() {
   return odom;
 }
 
+void KalmanFilterEstimate::loadSettings(const std::string& taskFile, bool verbose) {
+  boost::property_tree::ptree pt;
+  boost::property_tree::read_info(taskFile, pt);
+  std::string prefix = "kalmanFilter.";
+  if (verbose) {
+    std::cerr << "\n #### Kalman Filter Noise:";
+    std::cerr << "\n #### =============================================================================\n";
+  }
+
+  loadData::loadPtreeValue(pt, footRadius_, prefix + "footRadius", verbose);
+  loadData::loadPtreeValue(pt, imuProcessNoisePosition_, prefix + "imuProcessNoisePosition", verbose);
+  loadData::loadPtreeValue(pt, imuProcessNoiseVelocity_, prefix + "imuProcessNoiseVelocity", verbose);
+  loadData::loadPtreeValue(pt, footProcessNoisePosition_, prefix + "footProcessNoisePosition", verbose);
+  loadData::loadPtreeValue(pt, footSensorNoisePosition_, prefix + "footSensorNoisePosition", verbose);
+  loadData::loadPtreeValue(pt, footSensorNoiseVelocity_, prefix + "footSensorNoiseVelocity", verbose);
+  loadData::loadPtreeValue(pt, footHeightSensorNoise_, prefix + "footHeightSensorNoise", verbose);
+}
+
 }  // namespace legged