From 38ebedc27f375d4a29ddff64e584644381b63585 Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 11:39:20 -0500 Subject: [PATCH 01/11] Create ConstVelKF.cpp --- ff_estimate/ConstVelKF.cpp | 158 +++++++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 ff_estimate/ConstVelKF.cpp diff --git a/ff_estimate/ConstVelKF.cpp b/ff_estimate/ConstVelKF.cpp new file mode 100644 index 0000000..be281f5 --- /dev/null +++ b/ff_estimate/ConstVelKF.cpp @@ -0,0 +1,158 @@ +//Dock node and Filter + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std + +class ConstantVelKF { + public: + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + double MAX_DT; + + ConstantAccelKF(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; + } + + double wrap_angle(double theta) { + return atan2(sin(theta), cos(theta)); + } + + private: + Eigen::VectorXd x; + Eigen::MatrixXd P; + int dim; + int angle_idx; +}; + + +#include "ff_estimate/base_mocap_estimator.hpp" + +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; + } + + void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { + FreeFlyerState state{}; + Pose2D pose2d{}; + + 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(); + + 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; + } + + 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 est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { + if (!target_pose_.has_value()) { + return; + } + + 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); + } + + 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(); +} From dfc683f0ba32f3aeddb75678a9a2efd5b3abc0eb Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 11:40:33 -0500 Subject: [PATCH 02/11] Create ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 158 +++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 ff_estimate/src/ConstVelKF.cpp diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp new file mode 100644 index 0000000..be281f5 --- /dev/null +++ b/ff_estimate/src/ConstVelKF.cpp @@ -0,0 +1,158 @@ +//Dock node and Filter + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std + +class ConstantVelKF { + public: + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + double MAX_DT; + + ConstantAccelKF(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; + } + + double wrap_angle(double theta) { + return atan2(sin(theta), cos(theta)); + } + + private: + Eigen::VectorXd x; + Eigen::MatrixXd P; + int dim; + int angle_idx; +}; + + +#include "ff_estimate/base_mocap_estimator.hpp" + +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; + } + + void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { + FreeFlyerState state{}; + Pose2D pose2d{}; + + 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(); + + 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; + } + + 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 est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { + if (!target_pose_.has_value()) { + return; + } + + 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); + } + + 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(); +} From e7649e4fb0c23b7b7723466e9d05d2fd83a73bff Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 11:41:44 -0500 Subject: [PATCH 03/11] Update CMakeLists.txt added support for kalman filter --- ff_estimate/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ff_estimate/CMakeLists.txt b/ff_estimate/CMakeLists.txt index a22e694..19b06a5 100644 --- a/ff_estimate/CMakeLists.txt +++ b/ff_estimate/CMakeLists.txt @@ -43,6 +43,10 @@ install( add_executable(moving_avg_estimator_node src/moving_avg_estimator_node.cpp) target_link_libraries(moving_avg_estimator_node est_lib) +#add Kalman filter node +add_executable(ConstVelKF src/ConstVelKF.cpp) +target_link_libraries(ConstVelKF est_lib) + install(TARGETS moving_avg_estimator_node DESTINATION lib/${PROJECT_NAME}) From 7edea74c4e7063ef833acb5de28763e01fb8ddfc Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 12:20:57 -0500 Subject: [PATCH 04/11] Update ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index be281f5..156fc2a 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -1,14 +1,14 @@ //Dock node and Filter +#include "ff_estimate/base_mocap_estimator.hpp" + -#include -#include -#include -#include -#include -#include -#include -#include -#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 From b5981fe58e197d8f0bf0da5ef54a254f9143277d Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 13:18:16 -0500 Subject: [PATCH 05/11] Update ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index 156fc2a..e7b3547 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -10,7 +10,7 @@ using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::Pose2DStamped; using ff_msgs::msg::Pose2D; -using namespace std +using namespace std; class ConstantVelKF { public: @@ -18,7 +18,7 @@ class ConstantVelKF { Eigen::MatrixXd R; double MAX_DT; - ConstantAccelKF(Eigen::VectorXd x0, Eigen::MatrixXd P0, int dim = 3, int angle_idx = 2) + 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; 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 06/11] 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(); } +// From 0982a1fb1e01774bb5bbfdcdeee6336f99a2df22 Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 18:16:32 -0500 Subject: [PATCH 07/11] Update ConstVelKF.cpp --- ff_estimate/ConstVelKF.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/ff_estimate/ConstVelKF.cpp b/ff_estimate/ConstVelKF.cpp index be281f5..e7b3547 100644 --- a/ff_estimate/ConstVelKF.cpp +++ b/ff_estimate/ConstVelKF.cpp @@ -1,16 +1,16 @@ //Dock node and Filter +#include "ff_estimate/base_mocap_estimator.hpp" + -#include -#include -#include -#include -#include -#include -#include -#include -#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 namespace std; class ConstantVelKF { public: @@ -18,7 +18,7 @@ class ConstantVelKF { Eigen::MatrixXd R; double MAX_DT; - ConstantAccelKF(Eigen::VectorXd x0, Eigen::MatrixXd P0, int dim = 3, int angle_idx = 2) + 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; From 1340730332f3737284b98bfed5bf50f41acd451b Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 18:18:37 -0500 Subject: [PATCH 08/11] Update ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 264 ++++++++++++++++----------------- 1 file changed, 129 insertions(+), 135 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index 7803a26..e7b3547 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -1,164 +1,158 @@ -// 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. - - +//Dock node and Filter #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::BinaryCommand; -using ff_msgs::msg::ThrusterCommand; +using ff_msgs::msg::Pose2D; + +using namespace std; -class KalmanFilter : public ff::BaseMocapEstimator -{ +class ConstantVelKF { public: - 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); + 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; } - 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); + double wrap_angle(double theta) { + return atan2(sin(theta), cos(theta)); } - 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 + private: + Eigen::VectorXd x; + Eigen::MatrixXd P; + int dim; + int angle_idx; +}; - Eigen::MatrixXd Ts = P.Ts, Ta = P.Ta, Cov_s = P.Cov_s; - Eigen::VectorXd predict_mean = Ts * mean_p + Ta * a; - Eigen::MatrixXd predict_cov = Ts * var_p * Ts.transpose() + Cov_s; +#include "ff_estimate/base_mocap_estimator.hpp" - return {predict_mean, predict_cov}; +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; } - 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; + void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { + FreeFlyerState state{}; + Pose2D pose2d{}; - 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; + 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(); - b.mean = mean_b; - b.cov = var_b; + 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; } - void EstimateWithPose2D(const Pose2DStamped & pose_stamped) override - { - FreeFlyerState state{}; + 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); + } - 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()) { + void est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { + if (!target_pose_.has_value()) { 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; } - prev_.state = state; - prev_.header = pose_stamped.header; - SendStateEstimate(state); + 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); } + + 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(); } -// From f3424339a69dcb3fdcf6987f59a53de79a4ed7fa Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 19 Feb 2024 18:19:23 -0500 Subject: [PATCH 09/11] Delete ff_estimate/ConstVelKF.cpp --- ff_estimate/ConstVelKF.cpp | 158 ------------------------------------- 1 file changed, 158 deletions(-) delete mode 100644 ff_estimate/ConstVelKF.cpp diff --git a/ff_estimate/ConstVelKF.cpp b/ff_estimate/ConstVelKF.cpp deleted file mode 100644 index e7b3547..0000000 --- a/ff_estimate/ConstVelKF.cpp +++ /dev/null @@ -1,158 +0,0 @@ -//Dock node and Filter -#include "ff_estimate/base_mocap_estimator.hpp" - - -#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; - -class ConstantVelKF { - 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; - } - - double wrap_angle(double theta) { - return atan2(sin(theta), cos(theta)); - } - - private: - Eigen::VectorXd x; - Eigen::MatrixXd P; - int dim; - int angle_idx; -}; - - -#include "ff_estimate/base_mocap_estimator.hpp" - -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; - } - - void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { - FreeFlyerState state{}; - Pose2D pose2d{}; - - 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(); - - 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; - } - - 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 est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { - if (!target_pose_.has_value()) { - return; - } - - 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); - } - - 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(); -} From 68c4a70e0ef7581cf882b3ad2a4e20d760415419 Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Sat, 24 Feb 2024 16:01:09 -0500 Subject: [PATCH 10/11] Update ConstVelKF.cpp Not yet complete iteration - still fixing the shift from separate classes to combining both into one class. Also confused how I would get a z vector for this? --- ff_estimate/src/ConstVelKF.cpp | 51 +++++++++++++++++++++++++++++----- 1 file changed, 44 insertions(+), 7 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index e7b3547..f6f186d 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -12,15 +12,52 @@ using ff_msgs::msg::Pose2D; using namespace std; -class ConstantVelKF { +class ConstVelKalmanFilterNode : public ff::BaseMocapEstimator { + public: - Eigen::MatrixXd Q; - Eigen::MatrixXd R; - double MAX_DT; + Eigen::Matrix Q = Eigen::Matrix<6,6>::Identity(6, 6); + Eigen::Matrix R { + {2.4445e-3, 0 , 0 }, + { 0 , 1.2527e-3, 0 }, + { 0 , 0 , 4.0482e-3}, + }; + double MAX_DT = 1e-3; + + ConstVelKalmanFilterNode() : ff::BaseMocapEstimator("const_vel_kalman_filter_node") { + this->declare_parameter("min_dt", 0.005); + } + + void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { + + //R = Eigen::Matrix<3,3>::Identity(3, 3) * 2.4445e-3, 1.2527e-3, 4.0482e-3; + + FreeFlyerState state{}; + Pose2D pose2d{}; + + 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(); + + if (dt < (this->get_parameter("min_dt").as_double())) { + return; + } + + } else { + prev_state_ready_ = true; + } + + prev_.state = state; + prev_.header = pose_stamped.header; + + SendStateEstimate(state); + } + 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); + Q = Eigen::Matrix<6,6>::Identity(2 * dim, 2 * dim); R = Eigen::MatrixXd::Identity(dim, dim) * 2.4445e-3, 1.2527e-3, 4.0482e-3; MAX_DT = 1e-3; } @@ -30,8 +67,8 @@ class ConstantVelKF { return; } - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2 * dim, 2 * dim); - A.block(0, dim, dim, dim) = Eigen::MatrixXd::Identity(dim, dim) * dt; + Eigen::Matrix A = Eigen::Matrix::Identity(6, 6); + A.block<3, 3>(0, 3) = Eigen::Matrix::Identity(3, 3) * dt; x = A * x; P = A * P * A.transpose() + Q * dt; From 6e3862ddb42d258327254b1dfe9a2cd9327b6fc8 Mon Sep 17 00:00:00 2001 From: SunFireKing <65515101+SunFireKing@users.noreply.github.com> Date: Mon, 15 Apr 2024 16:17:09 -0400 Subject: [PATCH 11/11] Update ConstVelKF.cpp --- ff_estimate/src/ConstVelKF.cpp | 278 ++++++++++++++++++--------------- 1 file changed, 155 insertions(+), 123 deletions(-) diff --git a/ff_estimate/src/ConstVelKF.cpp b/ff_estimate/src/ConstVelKF.cpp index f6f186d..97c8949 100644 --- a/ff_estimate/src/ConstVelKF.cpp +++ b/ff_estimate/src/ConstVelKF.cpp @@ -3,7 +3,8 @@ #include - +#include "ff_estimate/base_mocap_estimator.hpp" + using Eigen::MatrixXd; using Eigen::VectorXd; using ff_msgs::msg::FreeFlyerState; @@ -15,26 +16,62 @@ using namespace std; class ConstVelKalmanFilterNode : public ff::BaseMocapEstimator { public: - Eigen::Matrix Q = Eigen::Matrix<6,6>::Identity(6, 6); - Eigen::Matrix R { - {2.4445e-3, 0 , 0 }, - { 0 , 1.2527e-3, 0 }, - { 0 , 0 , 4.0482e-3}, - }; - double MAX_DT = 1e-3; - + using Qmatrix = Eigen::Matrix; + using Rmatrix Eigen::Matrix3d; + ConstVelKalmanFilterNode() : ff::BaseMocapEstimator("const_vel_kalman_filter_node") { this->declare_parameter("min_dt", 0.005); + P.setIdentity(); + Q = (Qmatrix() << 1e-5, 0, 0, 0, 0, 0, //Process Noise Covariance Matrix + 0, 1e-5, 0, 0, 0, 0, + 0, 0, 1e-5, 0, 0, 0, + 0, 0, 0, 1e-3, 0, 0, + 0, 0, 0, 0, 1e-3, 0, + 0, 0, 0, 0, 0, 1e-3 + ).finished(); + + R = (Rmatrix() << 2.4445e-3, 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , + 0.0 , 1.2527e-3, 0.0 , 0.0 , 0.0 , 0.0 , + 0.0 , 0.0 , 4.0482e-3, 0.0 , 0.0 , 0.0 , + 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , + 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , + 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 + ).finished(); + + flag = 0; } void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { + FreeFlyerState state{}; + //initial declaration cycle + if (flag == 0) { + + Eigen::Vector3d pose = Eigen::Map(pose_stamped.pose, 3); + x.block<3, 1>(0,0) = pose; + flag++; + } else { + Eigen::Vector3d pose = Eigen::Map(pose_stamped.pose, 3); + prev_.header = pose_stamped.header; + const rclcpp::Time now = pose_stamped.header.stamp; + const rclcpp::Time last = prev_.header.stamp; + double dt = (now - last).seconds(); + process_update(dt); + measurement_update(pose); + state.pose.x = x.block<1, 1>(0,0); + state.pose.y = x.block<1, 1>(1,0); + state.pose.theta = x.block<1, 1>(2,0); + SendStateEstimate(state); + + } //R = Eigen::Matrix<3,3>::Identity(3, 3) * 2.4445e-3, 1.2527e-3, 4.0482e-3; - FreeFlyerState state{}; - Pose2D pose2d{}; + - state.pose = pose_stamped.pose; + + /* In order to use new state prediction, velocity must be discovered, and because there is constant vel + We can do (current state - previous state)/ change in time + if (prev_state_ready_) { const rclcpp::Time now = pose_stamped.header.stamp; const rclcpp::Time last = prev_.header.stamp; @@ -43,6 +80,28 @@ class ConstVelKalmanFilterNode : public ff::BaseMocapEstimator { if (dt < (this->get_parameter("min_dt").as_double())) { return; } + + double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; + double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; + + // wrap angle delta to [-pi, pi] + double dtheta = std::remainder(pose_stamped.pose.theta - prev_.state.pose.theta, 2 * M_PI); + double wz = dtheta / dt; + + //estimated velocities for naive state estimation + Eigen::Vector3d vel(vx, vy, wz); + + //get position vector from pose_stamped where vector is pose + //not sure if this is how to get the positions into an Eigen vector + Eigen::Vector3d pose = Eigen::Map(pose_stamped.pose, 3); + + //combine position vector and velocity vector for initial state vector + Eigen::Matrix x = Eigen::Matrix::Zero(6, 1); + x.block<3, 1>(3,0) = vel; + x.block<3, 1>(0,0) = pose; + + + } else { prev_state_ready_ = true; @@ -50,142 +109,115 @@ class ConstVelKalmanFilterNode : public ff::BaseMocapEstimator { prev_.state = state; prev_.header = pose_stamped.header; - - SendStateEstimate(state); + */ + //SendStateEstimate(state); } - 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::Matrix<6,6>::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; } - + /*A matrix used for prediction of next state matrix -- just for factoring in naive estimation into the + next position state matrix + Format: { + { 1 0 0 d 0 0} + { 0 1 0 0 d 0} + { 0 0 1 0 0 d} + { 0 0 0 1 0 0} + { 0 0 0 0 1 0} + { 0 0 0 0 0 1} + } + where d = dt + */ Eigen::Matrix A = Eigen::Matrix::Identity(6, 6); - A.block<3, 3>(0, 3) = Eigen::Matrix::Identity(3, 3) * dt; + A.block<3, 3>(0, 3).setIdentity() * dt; + //[6 x 6] * [6 x 1] = [6 x 1] x = A * x; + // P = [6 x 6] * [6 x 6] * [6 x 6] + [6 x 6] 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; + /*H matrix + Format:{ + { 1 0 0 0 0 0} + { 0 1 0 0 0 0} + { 0 0 1 0 0 0} + } + */ + Eigen::Matrix H = Eigen::Matrix::Zero(3, 6); + H.block<3, 3>(0, 0).setIdentity(); + /* S is divisor for Kalman Gain separated to be able to use inverse function + H * P * H.inv + R + [3 x 6] * [6 x 6] * [6 x 3] + [3 x 3] + [3 x 6] * [6 x 3] + [3 x 3] + [3 x 3] + [3 x 3] + S = [3 x 3] + */ + Eigen::Matrix3d S = (H * P) * H.transpose() + R; + /* K is kalman gain + K = P * H.inv / S + [6 x 6] * [6 x 3] / [3 x 3] + [6 x 3] / [3 x 3] + K = [6 x 3] + */ + Eigen::Matrix3d K = P * H.transpose() * S.inverse(); + /* y = [3 x 1] - [3 x 6] * [6 x 1] + [3 x 1] - [3 x 1] + z.block<1,1>(1,0) = wrap_angle(z.block<1,1>(1,0)); + Eigen::Matrix y = z - H * x; + y.block<1,1>(1,0) = wrap_angle(y.block<1,1>(1,0)); + /* x = [6 x 1] + [6 x 3] * ([3 x 1]) + [6 x 1] + [6 x 3] *([3 x 1]) + [6 x 1] + [6 x 1] + */ + //New State x + x = x + K * y; + + /* P = ([6 x 6] - [6 x 6] * [6 x 6]) * [6 x 6] + [6 x 6] * [6 x 6] + P = [6 x 6] + */ + //I is identity matrix <6 , 6> + Eigen::Matrix I = Eigen::Matrix::Identity(6, 6); + /* + P = [(6 x 6) - (6 x 3) * (3 x 6)] * (6 x 6) + */ + P = (I - K * H) * P; } double wrap_angle(double theta) { return atan2(sin(theta), cos(theta)); } +private: + Eigen::Matrix x = Eigen::Matrix::Zero(6, 1); - private: - Eigen::VectorXd x; - Eigen::MatrixXd P; - int dim; - int angle_idx; -}; - - -#include "ff_estimate/base_mocap_estimator.hpp" + /*Find out state variance-covariance matrix = P-- using identity matrix for now + 6 x 6 because of three states and three velocities for each of those states + P Format: + 1 0 0 0 0 0 + 0 1 0 0 0 0 + 0 0 1 0 0 0 + 0 0 0 1 0 0 + 0 0 0 0 1 0 + 0 0 0 0 0 1 + */ + Eigen::Matrix P = Eigen::Matrix::Identity(6, 6); -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; - } + //6 x 6 accounts for velocity, so even though we aren't fed in velocity we can calculate it and already have a variance for it + const Eigen::Matrix Q; - void EstimatewithPose2D(const Pose2DStamped & pose_stamped) override { - FreeFlyerState state{}; - Pose2D pose2d{}; + // R is Measurement Covariance Matrix. Can be thought of as observation error + const Eigen::Matrix R; + + + //double MAX_DT = 1e-3; - 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(); + int flag; - 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; - } - - 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 est_callback(const geometry_msgs::msg::Pose2DStamped::SharedPtr cv_pose) { - if (!target_pose_.has_value()) { - return; - } - - 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); - } - - 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) {