From b6f2ef76d6b50dd66beeee56032252fbc5ba54dc Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 13:28:54 -0500 Subject: [PATCH] Update ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 264 +++++++++++++++++---------------- 1 file changed, 135 insertions(+), 129 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index e7b3547..7803a26 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -1,158 +1,164 @@ -//Dock node and Filter -#include "ff_estimate/base_mocap_estimator.hpp" +// MIT License +// +// Copyright (c) 2023 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "ff_estimate/base_mocap_estimator.hpp" +#include #include using Eigen::MatrixXd; using Eigen::VectorXd; using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::Pose2DStamped; -using ff_msgs::msg::Pose2D; - -using namespace std; +using ff_msgs::msg::BinaryCommand; +using ff_msgs::msg::ThrusterCommand; -class ConstantVelKF { +class KalmanFilter : public ff::BaseMocapEstimator +{ public: - Eigen::MatrixXd Q; - Eigen::MatrixXd R; - double MAX_DT; - - ConstantVelKF(Eigen::VectorXd x0, Eigen::MatrixXd P0, int dim = 3, int angle_idx = 2) - : x(x0), P(P0), dim(dim), angle_idx(angle_idx) { - Q = Eigen::MatrixXd::Identity(2 * dim, 2 * dim); - R = Eigen::MatrixXd::Identity(dim, dim) * 2.4445e-3, 1.2527e-3, 4.0482e-3; - MAX_DT = 1e-3; - } - - void process_update(double dt) { - if (dt <= 0.) { - return; - } - - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2 * dim, 2 * dim); - A.block(0, dim, dim, dim) = Eigen::MatrixXd::Identity(dim, dim) * dt; - - x = A * x; - P = A * P * A.transpose() + Q * dt; - } - - void measurement_update(Eigen::VectorXd z) { - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(dim, 2 * dim); - H.block(0, 0, dim, dim) = Eigen::MatrixXd::Identity(dim, dim); - - Eigen::MatrixXd S = H * P * H.transpose() + R; - Eigen::MatrixXd K = P * H.transpose() * S.inverse(); - Eigen::VectorXd y = z - H * x; - y(angle_idx) = wrap_angle(y(angle_idx)); - - x += K * y; - P -= K * H * P; + KalmanFilter() : ff::BaseMocapEstimator("kalman_filter") + { + // kalman filtering on state tracking + this->declate_parameter("State_Transition", 1); + this->declare_parameter("Action_Transition", 1); + this->declare_parameter("predicted_covariance",1); } - double wrap_angle(double theta) { - return atan2(sin(theta), cos(theta)); + struct KalmanFilter { + Eigen::VectorXd mean; // mean belief vector: x, y, theta + Eigen::MatrixXd cov; // covariance of the belief + + KalmanFilter(const Eigen::VectorXd& initial_mean, const Eigen::MatrixXd& initial_cov) + : mean(initial_mean), cov(initial_cov) {} + }; + + struct pomdp{ + Eigen::VectorXd Ts; // Transition vector state + Eigen::VectorXd Ta; // Transition vector action + Eigen::MatrixXd Cov_s; // state transition covariance + Eigen::MatrixXd Cov_o; // observation covariance + Eigen::MatrixXd Os; // observation + pomdp(const Eigen::VectorXd& initial_Ts, + const Eigen::VectorXd& initial_Ta, + const Eigen::MatrixXd& initial_Cov_s, + const Eigen::MatrixXd& initial_Cov_o, + const Eigen::MatrixXd& initial_Os) + : Ts(initial_Ts), + Ta(initial_Ta), + Cov_s(initial_Cov_s), + Cov_o(initial_Cov_o), + Os(initial_Os) {} + }; + + void update(KalmanFilter& b, pomdp& P, const Eigen::VectorXd& a, const Eigen::VectorXd& o) + { + auto mean_var = Kalman_predict(b, P, a); + Kalman_update(b, P, o, mean_var.first, mean_var.second); } - private: - Eigen::VectorXd x; - Eigen::MatrixXd P; - int dim; - int angle_idx; -}; + std::pair Kalman_predict(const KalmanFilter& b, const pomdp& P, const Eigen::VectorXd& a) + { + Eigen::VectorXd mean_p = b.mean; // predicted mean + Eigen::MatrixXd var_p = b.cov; // predicted covariance + Eigen::MatrixXd Ts = P.Ts, Ta = P.Ta, Cov_s = P.Cov_s; -#include "ff_estimate/base_mocap_estimator.hpp" + Eigen::VectorXd predict_mean = Ts * mean_p + Ta * a; + Eigen::MatrixXd predict_cov = Ts * var_p * Ts.transpose() + Cov_s; -class ConstVelKalmanFilterNode : public ff::BaseMocapEstimator { - public: - ConstVelKalmanFilterNode() : ff::BaseMocapEstimator("const_vel_kalman_filter_node") { - this->declare_parameter("min_dt", 0.005); - this->target_pose = Pose2D; + return {predict_mean, predict_cov}; } - void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { - FreeFlyerState state{}; - Pose2D pose2d{}; + void Kalman_update(KalmanFilter& b, const pomdp& P, const Eigen::VectorXd& o, const Eigen::VectorXd& mean, const Eigen::MatrixXd& var) + { + Eigen::MatrixXd Sig_obs = P.Cov_o; + Eigen::MatrixXd Os = P.Os; - state.pose = pose_stamped.pose; - if (prev_state_ready_) { - const rclcpp::Time now = pose_stamped.header.stamp; - const rclcpp::Time last = prev_.header.stamp; - double dt = (now - last).seconds(); + Eigen::MatrixXd Kalman_gain = var * Sig_obs / (Os * var * Os.transpose() + Sig_obs); + Eigen::VectorXd mean_b = mean + Kalman_gain * (o - Os * mean); + Eigen::MatrixXd var_b = (Eigen::MatrixXd::Identity(3, 3) - Kalman_gain * Os) * var; - if (dt < (this->get_parameter("min_dt").as_double())) { - return; - } - - target_pose.pose.x = pose2d.pose.position.x; - target_pose.pose.y = pose2d.pose.position.y; - target_pose.pose.theta = pose2d.pose.position.theta; - - state.header = est_state.header - state.state.twist = pose_stamped.state.twist; - state.state.pose.x = this.target_pose.x; - state.state.pose.y = this.target_pose.y; - state.state.pose.theta = this.target_pose.theta; - } else { - prev_state_ready_ = true; + b.mean = mean_b; + b.cov = var_b; } - prev_.state = state; - prev_.header = pose_stamped.header; - - SendStateEstimate(state); - } - - private: - geometry_msgs::msg::TwistStamped; - geometry_msgs::msg::Pose2DStamped; - ff_msgs::msg::FreeFlyerStateStamped prev_; - bool prev_state_ready_ = false; - geometry_msgs::msg::Pose2D; - - /* void target_loop() { - if (!target_pose_.has_value()) { - return; - } - - auto target = std::make_shared(); - target->header.stamp = now(); - target->twist.linear.x = target_pose_->x; - target->twist.linear.y = target_pose_->y - 0.5; - target->twist.angular.z = target_pose_->theta; - - target_pub_->publish(target); - } + void EstimateWithPose2D(const Pose2DStamped & pose_stamped) override + { + FreeFlyerState state{}; - void est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { - if (!target_pose_.has_value()) { + if (prev_state_ready_) { + const rclcpp::Time now = pose_stamped.header.stamp; + const rclcpp::Time last = prev_.header.stamp; + double dt = (now - last).seconds(); + // ignore this frame if it is too close to the last frame + if (dt < this->get_parameter("min_dt").as_double()) { return; + } + Eigen::VectorXd pose = Eigen::Map(pose_stamped.pose, 3); + double Fmax = 0.6; + double dist = 1; + + Eigen::MatrixXd cov_b(3,3); + cov_b << 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0; + KalmanFilter KF(pose, cov_b); + + pomdp P( + Eigen::VectorXd(3) << 0.1, 0.1, 0.1, + Eigen::VectorXd(3) << 0.1, 0.1, 0.1, + Eigen::MatrixXd::Identity(3, 3), + Eigen::MatrixXd::Identity(3, 3), + Eigen::MatrixXd(3, 3) << 0.1, 0.1, 0.1 + ); + + update(KF, P, action, observation); + + state.pose = Eigen::VectorXd(3) << KF.mean[0], KF.mean[1], KF.mean[2]; + + // finite difference + double vx = (state.pose.x - prev_.state.pose.x) / dt; + double vy = (state.pose.y - prev_.state.pose.y) / dt; + // wrap angle delta to [-pi, pi] + double dtheta = std::remainder(state.pose.theta - prev_.state.pose.theta, 2 * M_PI); + double wz = dtheta / dt; + + double alpha = this->get_parameter("lowpass_coeff").as_double(); + if (alpha < 0 || alpha >= 1) { + RCLCPP_ERROR(this->get_logger(), "IIR filter disabled: invalid coefficient %f", alpha); + alpha = 0; + } + + state.twist.vx = alpha * prev_.state.twist.vx + (1. - alpha) * vx; + state.twist.vy = alpha * prev_.state.twist.vy + (1. - alpha) * vy; + state.twist.wz = alpha * prev_.state.twist.wz + (1. - alpha) * wz; + } else { + prev_state_ready_ = true; } - auto state = std::make_shared(); - state->header = cv_pose->header; - state->twist = cv_pose->pose; - state->twist.linear.x += target_pose_->x; - state->twist.linear.y += target_pose_->y; - state->twist.angular.z += target_pose_->theta; - - state_pub_->publish(state); + prev_.state = state; + prev_.header = pose_stamped.header; + SendStateEstimate(state); } - - void target_callback(const geometry_msgs::msg::PoseStamped::SharedPtr target_pose) { - if (!target_pose_.has_value()) { - target_pose_ = geometry_msgs::msg::Pose2D(); - } - - target_pose_->x = target_pose->pose.position.x; - target_pose_->y = target_pose->pose.position.y; - target_pose_->theta = M_PI / 2.0; - } */ -}; - -int main(int argc, char ** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); } +//