From 704401c45dda37258f046d711f3921d19d72d15b Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 12 Aug 2021 10:57:18 +0200
Subject: [PATCH] Automatic window size for sliding window filtering +
Possibility to filter angular velocity (off for now) + v_filt_bis_ is now of
size 6 to handle angular part
---
include/qrw/Estimator.hpp | 10 +++++-----
src/Estimator.cpp | 25 ++++++++++++++++++++-----
2 files changed, 25 insertions(+), 10 deletions(-)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 219d0b85..0bdb0a01 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -190,7 +190,7 @@ class Estimator {
VectorN getQFilt() { return q_filt_dyn_; }
VectorN getVFilt() { return v_filt_dyn_; }
VectorN getVSecu() { return v_secu_dyn_; }
- Vector3 getVFiltBis() { return v_filt_bis_; }
+ VectorN getVFiltBis() { return v_filt_bis_; }
Vector3 getRPY() { return IMU_RPY_; }
MatrixN getFeetStatus() { return feet_status_; }
MatrixN getFeetGoals() { return feet_goals_; }
@@ -212,7 +212,7 @@ class Estimator {
VectorN getQUpdated() { return q_up_; }
VectorN getVRef() { return v_ref_; }
VectorN getHV() { return h_v_; }
- Vector3 getHVBis() { return h_v_bis_; }
+ VectorN getHVBis() { return h_v_bis_; }
Matrix3 getoRh() { return oRh_; }
Vector3 getoTh() { return oTh_; }
double getYawEstim() { return yaw_estim_; }
@@ -273,8 +273,8 @@ class Estimator {
double yaw_estim_; // Yaw angle in perfect world
int N_queue_;
- Vector3 v_filt_bis_;
- Vector3 h_v_bis_;
- std::deque<double> vx_queue_, vy_queue_, vz_queue_;
+ VectorN v_filt_bis_;
+ VectorN h_v_bis_;
+ std::deque<double> vx_queue_, vy_queue_, vz_queue_, wR_queue_, wP_queue_, wY_queue_;
};
#endif // ESTIMATOR_H_INCLUDED
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 1757eb65..5417f3a0 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -76,8 +76,8 @@ Estimator::Estimator()
oTh_(Vector3::Zero()),
yaw_estim_(0.0),
N_queue_(0),
- v_filt_bis_(Vector3::Zero()),
- h_v_bis_(Vector3::Zero()) {}
+ v_filt_bis_(VectorN::Zero(6, 1)),
+ h_v_bis_(VectorN::Zero(6, 1)) {}
void Estimator::initialize(Params& params) {
dt_wbc = params.dt_wbc;
@@ -88,11 +88,15 @@ void Estimator::initialize(Params& params) {
double fc = params.fc_v_esti; // Cut frequency
double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
alpha_v_ = -y + std::sqrt(y * y + 2 * y);
+ alpha_v_ = 1.0;
- N_queue_ = static_cast<int>(std::round(1.0 / (dt_wbc * 3.0)));
+ N_queue_ = static_cast<int>(std::round(params.T_gait / dt_wbc));
vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
vz_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ wR_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ wP_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ wY_queue_.resize(N_queue_, 0.0); // List full of 0.0
// Filtering velocities used for security checks
fc = 6.0; // Cut frequency
@@ -356,7 +360,17 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
-
+ /*
+ wR_queue_.pop_back();
+ wP_queue_.pop_back();
+ wY_queue_.pop_back();
+ wR_queue_.push_front(filt_ang_vel(0));
+ wP_queue_.push_front(filt_ang_vel(1));
+ wY_queue_.push_front(filt_ang_vel(2));
+ v_filt_bis_(3) = std::accumulate(wR_queue_.begin(), wR_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(4) = std::accumulate(wP_queue_.begin(), wP_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(5) = std::accumulate(wY_queue_.begin(), wY_queue_.end(), 0.0) / N_queue_;*/
+ v_filt_bis_.tail(3) = filt_ang_vel; // No filtering for angular velocity
//////
// Update model used for the forward kinematics
@@ -426,7 +440,8 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
- h_v_bis_ = hRb * v_filt_bis_;
+ h_v_bis_.head(3) = hRb * v_filt_bis_.block(0, 0, 3, 1);
+ h_v_bis_.tail(3) = hRb * v_filt_bis_.block(3, 0, 3, 1);
} else {
// TODO: Adapt static mode to new version of the code
}
--
GitLab