HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update Kalman Filter

Browse Source
This commit is contained in:
HiepLM
2026-03-19 09:40:32 +07:00
parent ddb7df7c50
commit f0d987da39
6 changed files with 90 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
config/pnkx_local_planner_params.yaml
View File

@@ -79,6 +79,14 @@ MKTAlgorithmDiffPredictiveTrajectory:
index_samples: 60
follow_step_path: true
# Kalman filter tuning (filters v and w commands)
kf_q_v: 0.25
kf_q_w: 0.8
kf_r_v: 0.05
kf_r_w: 0.08
kf_p0: 0.5
kf_filter_angular: false
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:

2
examples/NavigationExample/NavigationExample.csproj
View File

@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net10.0</TargetFramework>
<TargetFramework>net6.0</TargetFramework>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

27
src/Algorithms/Libraries/kalman/src/kalman.cpp
View File

@@ -55,11 +55,34 @@ void KalmanFilter::update(const Eigen::VectorXd& y) {
if(!initialized)
throw std::runtime_error("Filter is not initialized!");
if (y.size() != m)
throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size");
if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n ||
Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m ||
P.rows() != n || P.cols() != n)
throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent");
x_hat_new = A * x_hat;
P = A*P*A.transpose() + Q;
K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
const Eigen::MatrixXd S = C * P * C.transpose() + R;
Eigen::LDLT<Eigen::MatrixXd> ldlt(S);
if (ldlt.info() != Eigen::Success)
throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance");
// K = P*C' * inv(S) -> solve(S * X = (P*C')^T)
const Eigen::MatrixXd PCt = P * C.transpose();
const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose());
if (ldlt.info() != Eigen::Success)
throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain");
K = KT.transpose();
x_hat_new += K * (y - C*x_hat_new);
P = (I - K*C)*P;
// Joseph form: keeps P symmetric / PSD under numeric errors
const Eigen::MatrixXd IKC = I - K * C;
P = IKC * P * IKC.transpose() + K * R * K.transpose();
x_hat = x_hat_new;
t += dt;

8
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -412,6 +412,14 @@ namespace mkt_algorithm
Eigen::MatrixXd Q;
Eigen::MatrixXd R;
Eigen::MatrixXd P;
// Kalman filter tuning (for v and w filtering)
double kf_q_v_;
double kf_q_w_;
double kf_r_v_;
double kf_r_w_;
double kf_p0_;
bool kf_filter_angular_;
#ifdef BUILD_WITH_ROS
ros::Publisher lookahead_point_pub_;
#endif

79
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -35,6 +35,12 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
cost_scaling_gain_(0.0),
control_duration_(0.0),
kf_(nullptr),
kf_q_v_(0.25),
kf_q_w_(0.8),
kf_r_v_(0.05),
kf_r_w_(0.08),
kf_p0_(0.5),
kf_filter_angular_(false),
m_(0),
n_(0)
{
@@ -83,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
// kalman
last_actuator_update_ = robot::Time::now();
n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
m_ = 2; // measurements: x, y, theta
// State: [v, a, j, w, alpha, jw] where:
// - v: linear velocity, a: linear accel, j: linear jerk
// - w: angular velocity, alpha: angular accel, jw: angular jerk
// Measurement: [v, w]
n_ = 6;
m_ = 2;
double dt = control_duration_;
// Khởi tạo ma trận
A = Eigen::MatrixXd::Identity(n_, n_);
C = Eigen::MatrixXd::Zero(m_, n_);
Q = Eigen::MatrixXd::Zero(n_, n_);
R = Eigen::MatrixXd::Identity(m_, m_);
P = Eigen::MatrixXd::Identity(n_, n_);
R = Eigen::MatrixXd::Zero(m_, m_);
P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
for (int i = 0; i < n_; i += 3)
{
@@ -103,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
C(0, 0) = 1;
C(1, 3) = 1;
Q(2, 2) = 0.1;
Q(5, 5) = 0.6;
Q(2, 2) = std::max(1e-12, kf_q_v_);
Q(5, 5) = std::max(1e-12, kf_q_w_);
R(0, 0) = 0.1;
R(1, 1) = 0.2;
P(3, 3) = 0.4;
P(4, 4) = 0.4;
P(5, 5) = 0.4;
R(0, 0) = std::max(1e-12, kf_r_v_);
R(1, 1) = std::max(1e-12, kf_r_w_);
kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
Eigen::VectorXd x0(n_);
@@ -177,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
use_cost_regulated_linear_velocity_scaling_ = false;
}
// Kalman filter tuning (filtering v and w commands)
nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);
double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
@@ -558,25 +573,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
result.velocity = drive_cmd;
return result;
}
// Eigen::VectorXd y(2);
// y << drive_cmd.x, drive_cmd.theta;
// // Cập nhật lại A nếu dt thay đổi
// for (int i = 0; i < n_; ++i)
// for (int j = 0; j < n_; ++j)
// A(i, j) = (i == j ? 1.0 : 0.0);
// for (int i = 0; i < n_; i += 3)
// {
// A(i, i + 1) = dt;
// A(i, i + 2) = 0.5 * dt * dt;
// A(i + 1, i + 2) = dt;
// }
// kf_->update(y, dt, A);
// double v_min = min_approach_linear_velocity_;
// drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
// drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
// drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
}
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
@@ -687,6 +683,27 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// Cập nhật lại A nếu dt thay đổi
for (int i = 0; i < n_; ++i)
for (int j = 0; j < n_; ++j)
A(i, j) = (i == j ? 1.0 : 0.0);
for (int i = 0; i < n_; i += 3)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
}
kf_->update(y, dt, A);
double v_min = min_approach_linear_velocity_;
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
if (kf_filter_angular_)
drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(