Skip to content

Commit

Permalink
Update ConstVelKF.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@SunFireKing
SunFireKing authored Feb 19, 2024
1 parent 0982a1f commit 1340730
Showing 1 changed file with 129 additions and 135 deletions.
264 changes: 129 additions & 135 deletions ff_estimate/src/ConstVelKF.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <iostream>

#include <Eigen/Dense>

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<Eigen::VectorXd, Eigen::MatrixXd> 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<geometry_msgs::msg::TwistStamped>();
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<Eigen::VectorXd>(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<geometry_msgs::msg::TwistStamped>();
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<ConstantVelKalmanFilterNode>());
rclcpp::shutdown();
}
//

0 comments on commit 1340730

Please sign in to comment.