用C++和Eigen库手把手实现一个机器人定位的卡尔曼滤波器（附完整代码）

从零实现机器人定位卡尔曼滤波C与Eigen实战指南当我在第一次尝试为自主移动机器人实现定位系统时面对卡尔曼滤波的理论公式和实际代码实现之间的鸿沟曾一度陷入困境。本文将分享如何用C和Eigen库一步步构建一个完整的差分驱动机器人定位系统避开那些教科书上不会告诉你的实践陷阱。1. 环境准备与基础架构1.1 Eigen库的安装与配置Eigen是一个纯头文件的C模板库安装非常简单# 使用vcpkg安装 vcpkg install eigen3 # 或者直接下载头文件 wget https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz tar -xzf eigen-3.4.0.tar.gz在CMake项目中配置Eigenfind_package(Eigen3 REQUIRED) target_link_libraries(your_project Eigen3::Eigen)1.2 项目目录结构建议采用以下模块化结构robot_kalman_filter/ ├── include/ │ ├── system_model.hpp # 系统运动模型 │ ├── measurement_model.hpp # 测量模型 │ └── utils.hpp # 辅助函数 ├── src/ │ ├── main.cpp # 主程序 │ └── kalman_filter.cpp # 滤波器实现 ├── test/ # 单元测试 └── CMakeLists.txt2. 核心数学模型实现2.1 状态向量定义对于差分驱动机器人我们需要跟踪位置(x,y)和朝向θtemplatetypename T class RobotState : public Eigen::MatrixT, 3, 1 { public: using Base Eigen::MatrixT, 3, 1; using Base::Base; // 便捷访问方法 T x() const { return (*this)(0); } T y() const { return (*this)(1); } T theta() const { return (*this)(2); } T x() { return (*this)(0); } T y() { return (*this)(1); } T theta() { return (*this)(2); } // 状态维度 static constexpr int size 3; };2.2 运动模型实现基于差分驱动运动学实现预测步骤templatetypename T class DifferentialDriveModel { public: using State RobotStateT; using Control Eigen::MatrixT, 2, 1; // [v, ω] State predict(const State x, const Control u, T dt) const { State x_new; if (std::abs(u[1]) 1e-6) { // 处理直线运动 x_new.x() x.x() u[0] * std::cos(x.theta()) * dt; x_new.y() x.y() u[0] * std::sin(x.theta()) * dt; x_new.theta() x.theta(); } else { // 曲线运动 T radius u[0] / u[1]; x_new.theta() x.theta() u[1] * dt; x_new.x() x.x() radius * (std::sin(x_new.theta()) - std::sin(x.theta())); x_new.y() x.y() - radius * (std::cos(x_new.theta()) - std::cos(x.theta())); } return x_new; } Eigen::MatrixT, 3, 3 jacobianF(const State x, const Control u, T dt) const { Eigen::MatrixT, 3, 3 F Eigen::MatrixT, 3, 3::Identity(); if (std::abs(u[1]) 1e-6) { F(0, 2) -u[0] * std::sin(x.theta()) * dt; F(1, 2) u[0] * std::cos(x.theta()) * dt; } else { T radius u[0] / u[1]; F(0, 2) radius * (std::cos(x.theta() u[1] * dt) - std::cos(x.theta())); F(1, 2) radius * (std::sin(x.theta() u[1] * dt) - std::sin(x.theta())); } return F; } };3. 卡尔曼滤波核心实现3.1 预测步骤实现templatetypename T void predict(KalmanFilterT kf, const DifferentialDriveModelT model, const typename DifferentialDriveModelT::Control u, T dt, const Eigen::MatrixT, 3, 3 Q) { // 状态预测 kf.state model.predict(kf.state, u, dt); // 协方差预测 Eigen::MatrixT, 3, 3 F model.jacobianF(kf.state, u, dt); kf.covariance F * kf.covariance * F.transpose() Q; // 数值稳定性处理 kf.covariance 0.5 * (kf.covariance kf.covariance.transpose()); }3.2 更新步骤实现对于位置测量更新templatetypename T void update(KalmanFilterT kf, const Eigen::MatrixT, 2, 1 z, // [x_meas, y_meas] const Eigen::MatrixT, 2, 3 H, // 观测矩阵 const Eigen::MatrixT, 2, 2 R) { // 计算卡尔曼增益 Eigen::MatrixT, 2, 2 S H * kf.covariance * H.transpose() R; Eigen::MatrixT, 3, 2 K kf.covariance * H.transpose() * S.inverse(); // 状态更新 Eigen::MatrixT, 2, 1 z_pred H * kf.state; kf.state K * (z - z_pred); // 协方差更新 (Joseph形式保证正定性) Eigen::MatrixT, 3, 3 I Eigen::MatrixT, 3, 3::Identity(); kf.covariance (I - K * H) * kf.covariance * (I - K * H).transpose() K * R * K.transpose(); }4. 实战完整定位系统实现4.1 主循环架构int main() { // 初始化 KalmanFilterdouble kf; DifferentialDriveModeldouble model; // 初始状态 kf.state.x() 0.0; kf.state.y() 0.0; kf.state.theta() 0.0; kf.covariance.setIdentity(); // 噪声参数 Eigen::Matrix3d Q Eigen::Matrix3d::Identity() * 0.01; Eigen::Matrix2d R Eigen::Matrix2d::Identity() * 0.1; // 仿真参数 const double dt 0.1; const int steps 100; // 主循环 for (int i 0; i steps; i) { // 生成控制输入 (速度角速度) Eigen::Vector2d u(1.0, 0.1 * std::sin(i * 0.1)); // 预测步骤 predict(kf, model, u, dt, Q); // 模拟真实测量 (添加噪声) Eigen::Vector2d z; z[0] kf.state.x() normal_distribution(generator) * std::sqrt(R(0,0)); z[1] kf.state.y() normal_distribution(generator) * std::sqrt(R(1,1)); // 更新步骤 Eigen::Matrixdouble, 2, 3 H; H 1, 0, 0, 0, 1, 0; update(kf, z, H, R); // 输出结果 std::cout Step i : True( true_x , true_y ) Estimate( kf.state.x() , kf.state.y() ) std::endl; } return 0; }4.2 性能优化技巧矩阵运算优化// 不好的写法创建临时矩阵 Eigen::Matrix3d temp F * P; temp temp * F.transpose(); P temp Q; // 好的写法利用Eigen的表达式模板 P F * P * F.transpose() Q;内存预分配// 在循环外预先分配 Eigen::Matrix3d F; Eigen::Matrix2d S; Eigen::Matrixdouble, 3, 2 K; // 在循环内重用这些矩阵 F model.jacobianF(kf.state, u, dt); S H * P * H.transpose() R; K P * H.transpose() * S.inverse();5. 调试与验证方法5.1 一致性检验double calculateNIS(const Eigen::Vector2d z, const Eigen::Vector2d z_pred, const Eigen::Matrix2d S) { Eigen::Vector2d innovation z - z_pred; return innovation.transpose() * S.inverse() * innovation; } void checkConsistency(double nis, double chi_square_threshold) { if (nis chi_square_threshold) { std::cerr Warning: Filter may be inconsistent (NIS nis ) std::endl; } }5.2 可视化调试建议使用Python matplotlib进行结果可视化import matplotlib.pyplot as plt def plot_trajectory(true_states, estimated_states): plt.figure(figsize(10, 6)) plt.plot([x for x, y in true_states], [y for x, y in true_states], g-, labelTrue Path) plt.plot([x for x, y in estimated_states], [y for x, y in estimated_states], b--, labelEstimated) plt.xlabel(X position) plt.ylabel(Y position) plt.legend() plt.grid(True) plt.show()6. 进阶话题与扩展6.1 处理非线性测量模型当使用距离或角度传感器时需要扩展卡尔曼滤波templatetypename T class RangeBearingModel { public: Eigen::Vector2d h(const RobotStateT x, const Eigen::Vector2d landmark) const { Eigen::Vector2d z; double dx landmark[0] - x.x(); double dy landmark[1] - x.y(); z[0] std::sqrt(dx*dx dy*dy); // 距离 z[1] std::atan2(dy, dx) - x.theta(); // 方位角 // 归一化角度到[-π, π] while (z[1] M_PI) z[1] - 2*M_PI; while (z[1] -M_PI) z[1] 2*M_PI; return z; } Eigen::MatrixT, 2, 3 jacobianH(const RobotStateT x, const Eigen::Vector2d landmark) const { Eigen::MatrixT, 2, 3 H; double dx landmark[0] - x.x(); double dy landmark[1] - x.y(); double d_sq dx*dx dy*dy; double d std::sqrt(d_sq); // 距离对状态的导数 H(0, 0) -dx / d; // ∂d/∂x H(0, 1) -dy / d; // ∂d/∂y H(0, 2) 0; // ∂d/∂θ // 方位角对状态的导数 H(1, 0) dy / d_sq; // ∂ϕ/∂x H(1, 1) -dx / d_sq; // ∂ϕ/∂y H(1, 2) -1; // ∂ϕ/∂θ return H; } };6.2 多传感器融合void fuseMultipleSensors(KalmanFilterdouble kf, const std::vectorSensorReading readings) { for (const auto reading : readings) { switch (reading.type) { case SensorType::POSITION: updatePosition(kf, reading.data, R_position); break; case SensorType::IMU: updateOrientation(kf, reading.data, R_imu); break; case SensorType::LANDMARK: updateLandmark(kf, reading.data, R_landmark); break; } } }