From 38e8b96330b9b62236e268515c54c2da6357547c Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Wed, 14 Oct 2026 02:24:33 +0800 Subject: [PATCH 1/3] Add recursive least squares filter header. --- rm_common/include/rm_common/rls.h | 165 ++++++++++++++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 rm_common/include/rm_common/rls.h diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h new file mode 100644 index 00000000..cf0f2030 --- /dev/null +++ b/rm_common/include/rm_common/rls.h @@ -0,0 +1,165 @@ +// +// Created by xiezd on 3/25/26. +// + +#pragma once + +#include +#include +#include + +#include +#include "eigen_types.h" + +#define RLS_ASSERT(condition, message) \ + if (!(condition)) \ + throw std::invalid_argument(message) + +template +class Rls +{ +public: + Rls(int xSize, int ySize, T lambda, T pInit) : xSize_(xSize), ySize_(ySize), lambda_(lambda) + { + RLS_ASSERT(xSize_ > 0, "rls: xSize should be positive"); + RLS_ASSERT(ySize_ > 0, "rls: ySize should be positive"); + RLS_ASSERT(std::abs(lambda_) > std::numeric_limits::epsilon(), "rls: lambda should be non-zero"); + + x_.resize(xSize_, 1); + x_.setZero(); + xt_.resize(1, xSize_); + xt_.setZero(); + + w_.resize(xSize_, ySize_); + w_.setZero(); + output_.resize(xSize_, ySize_); + output_.setZero(); + k_.resize(xSize_, 1); + k_.setZero(); + kNumerator_.resize(xSize_, 1); + kNumerator_.setZero(); + + p_.resize(xSize_, xSize_); + p_.setIdentity(); + p_ *= pInit; + + y_.resize(ySize_, 1); + y_.setZero(); + u_.resize(ySize_, 1); + u_.setZero(); + e_.resize(ySize_, 1); + e_.setZero(); + } + + template + bool setX(const Eigen::MatrixBase& x) + { + static_assert(TX::ColsAtCompileTime == 1 || TX::ColsAtCompileTime == Eigen::Dynamic, + "rls: X should be a column vector"); + RLS_ASSERT(x.rows() == xSize_ && x.cols() == 1, "rls: X size mismatch"); + x_ = x; + return true; + } + + template + bool setY(const Eigen::MatrixBase& y) + { + static_assert(TY::ColsAtCompileTime == 1 || TY::ColsAtCompileTime == Eigen::Dynamic, + "rls: Y should be a column vector"); + RLS_ASSERT(y.rows() == ySize_ && y.cols() == 1, "rls: Y size mismatch"); + y_ = y; + return true; + } + + bool setY(T y) + { + RLS_ASSERT(ySize_ == 1, "rls: setY(scalar) requires ySize==1"); + y_(0, 0) = y; + return true; + } + + template + bool setW(const Eigen::MatrixBase& w) + { + RLS_ASSERT(w.rows() == xSize_ && w.cols() == ySize_, "rls: W size mismatch"); + w_ = w; + return true; + } + + template + bool setU(const Eigen::MatrixBase& u) + { + static_assert(TU::ColsAtCompileTime == 1 || TU::ColsAtCompileTime == Eigen::Dynamic, + "rls: U should be a column vector"); + RLS_ASSERT(u.rows() == ySize_ && u.cols() == 1, "rls: U size mismatch"); + u_ = u; + return true; + } + + bool setU(T u) + { + RLS_ASSERT(ySize_ == 1, "rls: setU(scalar) requires ySize==1"); + u_(0, 0) = u; + return true; + } + + bool update() + { + xt_ = x_.transpose(); + // Compute predicted output using current weights: u_pred = x^T * w + // Compute prediction error: e = y - u_pred + e_ = y_ - u_; + + kNumerator_ = p_ * x_; + T denominator = lambda_ + (xt_ * p_ * x_)(0, 0); + RLS_ASSERT(std::abs(denominator) > std::numeric_limits::epsilon(), + "rls: denominator too small, numerical instability"); + + // Compute gain vector: k = P*x / (lambda + x^T*P*x) + k_ = kNumerator_ / denominator; + // Update parameters: w = w + k * e^T + output_ = w_ + k_ * e_.transpose(); + w_ = output_; + + // Update covariance matrix: P = (P - k*x^T*P) / lambda + p_ = (p_ - k_ * xt_ * p_) / lambda_; + return true; + } + + DMat getW() const + { + return w_; + } + + DMat getP() const + { + return p_; + } + + DVec getError() const + { + return e_; + } + + DVec getEstimate() const + { + return u_; + } + +private: + int xSize_; + int ySize_; + T lambda_; + + DVec x_; + DMat xt_; + DMat w_; + DMat output_; + DVec k_; + DVec kNumerator_; + DMat p_; + + DVec y_; + DVec u_; + DVec e_; +}; From e2b657978e960b633e25949b8f7cadf6d71940cc Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Wed, 14 Oct 2026 02:28:46 +0800 Subject: [PATCH 2/3] Add numeric clamp helper. --- rm_common/include/rm_common/math_utilities.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rm_common/include/rm_common/math_utilities.h b/rm_common/include/rm_common/math_utilities.h index 841a5674..08aac317 100644 --- a/rm_common/include/rm_common/math_utilities.h +++ b/rm_common/include/rm_common/math_utilities.h @@ -76,3 +76,13 @@ T alpha(T cutoff, double freq) T te = 1.0 / freq; return 1.0 / (1.0 + tau / te); } + +template +T limit(T val, T min, T max) +{ + if (val > max) + return max; + if (val < min) + return min; + return val; +} From 872acea3656e2703af89a52be7ee0ee30408fbe9 Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Wed, 14 Oct 2026 22:08:55 +0800 Subject: [PATCH 3/3] Add zip_state field to ManualToReferee msg. --- rm_msgs/msg/referee/ManualToReferee.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_msgs/msg/referee/ManualToReferee.msg b/rm_msgs/msg/referee/ManualToReferee.msg index a51c9af6..4d7f9bc5 100644 --- a/rm_msgs/msg/referee/ManualToReferee.msg +++ b/rm_msgs/msg/referee/ManualToReferee.msg @@ -4,6 +4,7 @@ time start_burst_time #standard uint8 shoot_frequency bool cover_state +bool zip_state #hero bool gimbal_eject