diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 9b3aa9d..4783027 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -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: diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj index 9ec6097..c98bcda 100644 --- a/examples/NavigationExample/NavigationExample.csproj +++ b/examples/NavigationExample/NavigationExample.csproj @@ -1,7 +1,7 @@ Exe - net10.0 + net6.0 true diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index d225bf1..626ec96 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/src/Algorithms/Libraries/kalman/src/kalman.cpp b/src/Algorithms/Libraries/kalman/src/kalman.cpp index 4c44312..5e47d9f 100755 --- a/src/Algorithms/Libraries/kalman/src/kalman.cpp +++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp @@ -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 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; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index d7d2427..6a21147 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 9b2e510..1a7c0d9 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -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(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("kf_q_v", kf_q_v_, kf_q_v_); + nh_priv_.param("kf_q_w", kf_q_w_, kf_q_w_); + nh_priv_.param("kf_r_v", kf_r_v_, kf_r_v_); + nh_priv_.param("kf_r_w", kf_r_w_, kf_r_w_); + nh_priv_.param("kf_p0", kf_p0_, kf_p0_); + nh_priv_.param("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(