[实战] 卡尔曼滤波原理与实现（GITHUB 优秀库解读）

原创已于 2025-11-10 08:35:34 修改 · 1.1k 阅读

17 ·

CC 4.0 BY-SA版权

文章标签：

#github #卡尔曼滤波 #kalman filter #算法 #信号处理

于 2025-11-10 08:32:17 首次发布

卡尔曼滤波原理与实现

1.本文是对他山之玉的雕琢，代码来自Github：https://github.com/mherb/kalman.git
2.依赖的数学库Eigen地址：https://libeigen.gitlab.io/
3.他山之石，可以攻玉，希望本解读能给需要这带来一些帮助。
4.感谢为开源社区做贡献的各位大神

文章目录

卡尔曼滤波原理与实现
- 1. 理论基础
- - 1.1 卡尔曼滤波基本概念
  - 1.2 系统模型与测量模型
  - 1.3 预测与更新步骤
- 2. 工程结构
- - 2.1 项目整体结构
  - 2.2 核心类库继承关系
  - 2.3 Eigen数学库依赖分析
- 3. Kalman库核心实现分析
- - 3.1 基础模板类设计
  - 3.2 滤波器基类实现
- 4. Robot1案例详解
- - 4.1 系统状态定义
  - 4.2 控制输入定义
  - 4.3 系统模型实现
  - 4.4 位置测量模型
  - 4.5 主函数流程说明
- 5. Eigen数学库在Kalman滤波中的关键应用
- - 5.1 矩阵运算的数学原理
  - 5.2 状态向量运算
  - 5.3 协方差矩阵运算
  - 5.4 矩阵模板特化
  - 5.5 高效的矩阵运算
  - 5.6 数值稳定性保障
- 6. 工程实践与性能优化
- - 6.1 内存管理优化
  - 6.2 计算性能优化
- 7. 调试与验证方法论
- - 7.1 一致性检验
  - 7.2 蒙特卡洛仿真验证
- 8. 工程实践建议
- - 8.1 滤波器选择流程图
  - 8.2 调试策略
  - 8.3 性能优化技巧
- 9. 总结与扩展应用

1. 理论基础

1.1 卡尔曼滤波基本概念

卡尔曼滤波是一种递归的贝叶斯估计算法，用于从包含噪声的测量数据中估计动态系统的状态。其核心思想是通过结合系统模型预测和实际测量值，获得最优的状态估计。

1.2 系统模型与测量模型

系统模型（状态转移方程）：
$x_k = Fx_{k-1} + Bu_k + w_k$

测量模型（观测方程）：
$z_k = Hx_k + v_k$

其中：

$x_k$ ：系统在时刻k的状态向量
$F$ ：状态转移矩阵
$B$ ：控制输入矩阵
$u_k$ ：控制输入向量
$w_k$ ：系统噪声，服从 $N (0, Q)$
$z_k$ ：测量向量
$H$ ：观测矩阵
$v_k$ ：测量噪声，服从 $N (0, R)$

1.3 预测与更新步骤

预测步骤：

状态预测： $\hat{x}_{k|k-1} = F\hat{x}_{k-1|k-1} + Bu_k$
协方差预测： $P_{k|k-1} = FP_{k-1|k-1}F^T + Q$

更新步骤：

卡尔曼增益： $K_k = P_{k|k-1}H^T(HP_{k|k-1}H^T + R)^{-1}$
状态更新： $\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - H\hat{x}_{k|k-1})$
协方差更新： $P_{k|k} = (I - K_kH)P_{k|k-1}$

2. 工程结构

2.1 项目整体结构

2.2 核心类库继承关系

2.3 Eigen数学库依赖分析

Eigen-3.4.1库结构：

在Kalman库中的关键应用：

Matrix模板类：用于状态向量和协方差矩阵
动态大小矩阵：支持不同维度的状态空间
高效的矩阵运算：优化预测和更新计算
数值稳定性：提供可靠的矩阵分解算法

3. Kalman库核心实现分析

3.1 基础模板类设计

Kalman::Vector模板类：

// 基础向量模板，继承自Eigen::Matrix
template<typename T, int N>
class Vector : public Eigen::Matrix<T, N, 1>
{
public:
    // 提供便捷的访问接口
    template<int Index>
    T get() const { return (*this)[Index]; }
    
    template<int Index>
    void set(T value) { (*this)[Index] = value; }
};

KALMAN_VECTOR宏定义：

// 简化状态向量定义的宏
#define KALMAN_VECTOR(Name, Type, Size) \
    typedef Kalman::Vector<Type, Size> Base; \
    using Base::Base; \
    using Base::operator=; \
    static constexpr int size = Size;

3.2 滤波器基类实现

KalmanFilter基类关键方法：

template<typename State>
class KalmanFilter
{
protected:
    State x;  // 当前状态估计
    Covariance<State> P;  // 当前协方差矩阵

public:
    // 初始化滤波器
    void init(const State& initialState) {
        x = initialState;
        P = Covariance<State>::Identity();
    }
    
    // 获取当前状态
    const State& getState() const { return x; }
    
    // 获取当前协方差
    const Covariance<State>& getCovariance() const { return P; }
};

4. Robot1案例详解

4.1 系统状态定义

Robot1案例实现了一个3自由度机器人的定位问题，状态向量包含位置和方向：

// 状态向量定义：x, y, theta
class State : public Kalman::Vector<T, 3>
{
public:
    KALMAN_VECTOR(State, T, 3)
    // 状态分量访问
    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]; }
};

数学原理：状态向量 $\theta]^T$ 表示机器人在平面坐标系中的位置 $(x, y)$ 和朝向角度 $\theta$ 。

4.2 控制输入定义

控制输入包含线速度和角速度：

// 控制输入定义：v, dtheta
class Control : public Kalman::Vector<T, 2>
{
public:
    KALMAN_VECTOR(Control, T, 2)
    // 控制分量访问
    T v() const { return (*this)[0]; }
    T dtheta() const { return (*this)[1]; }
    
    T& v() { return (*this)[0]; }
    T& dtheta() { return (*this)[1]; }
};

数学原理：控制向量 $\dot{\theta}]^T$ ，其中 $v$ 为线速度， $\dot{\theta}$ 为角速度。

4.3 系统模型实现

系统模型定义了机器人的运动学方程：

template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class SystemModel : public Kalman::LinearizedSystemModel<State, Control, CovarianceBase>
{
public:
    // 状态转移函数
    State f(const State& x, const Control& u) const override
    {
        State x_;
        // 位置更新：x = x + v*cos(theta)*dt
        //           y = y + v*sin(theta)*dt  
        // 方向更新：theta = theta + dtheta*dt
        x_.x() = x.x() + u.v() * std::cos(x.theta()) * dt;
        x_.y() = x.y() + u.v() * std::sin(x.theta()) * dt;
        x_.theta() = x.theta() + u.dtheta() * dt;
        return x_;
    }
    
protected:
    // 更新雅可比矩阵
    void updateJacobians(const State& x, const Control& u) override
    {
        this->F.setIdentity();
        // 位置对位置的偏导：∂x/∂x = 1, ∂y/∂y = 1
        // 位置对方向的偏导：∂x/∂θ = -v*sin(θ)*dt, ∂y/∂θ = v*cos(θ)*dt
        this->F(0,2) = -u.v() * std::sin(x.theta()) * dt;
        this->F(1,2) = u.v() * std::cos(x.theta()) * dt;
        
        this->W.setIdentity();
    }
    
private:
    T dt = 1.0; // 时间步长
};

数学原理：系统模型基于差分驱动机器人的运动学方程：

位置更新： $x_{k+1} = x_k + v\cos(\theta_k)\Delta t$
方向更新： $\theta_{k+1} = \theta_k + \dot{\theta}\Delta t$

雅可比矩阵 $F$ 包含状态转移的线性化信息：
$\begin{bmatrix} 1 & 0 & -v\sin(\theta)\Delta t \\ 0 & 1 & v\cos(\theta)\Delta t \\ 0 & 0 & 1 \end{bmatrix}$

4.4 位置测量模型

位置测量模型基于两个固定landmark的距离测量：

template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class PositionMeasurementModel : public Kalman::LinearizedMeasurementModel<State, PositionMeasurement, CovarianceBase>
{
public:
    PositionMeasurementModel()
    {
        // 设置两个landmark的位置
        l1 << 0.0, 0.0;
        l2 << 10.0, 0.0;
    }
    
    // 测量函数：计算机器人到两个landmark的距离
    PositionMeasurement h(const State& x) const override
    {
        PositionMeasurement z;
        // 到第一个landmark的距离
        z.d1() = std::sqrt(std::pow(x.x() - l1[0], 2) + std::pow(x.y() - l1[1], 2));
        // 到第二个landmark的距离  
        z.d2() = std::sqrt(std::pow(x.x() - l2[0], 2) + std::pow(x.y() - l2[1], 2));
        return z;
    }
    
protected:
    // 更新雅可比矩阵
    void updateJacobians(const State& x) override
    {
        // 距离对位置的偏导数
        T dx1 = x.x() - l1[0];
        T dy1 = x.y() - l1[1];
        T d1 = std::sqrt(dx1*dx1 + dy1*dy1);
        
        T dx2 = x.x() - l2[0];
        T dy2 = x.y() - l2[1];
        T d2 = std::sqrt(dx2*dx2 + dy2*dy2);
        
        // H矩阵：测量对状态的偏导
        this->H(0,0) = dx1/d1;  // ∂d1/∂x
        this->H(0,1) = dy1/d1;  // ∂d1/∂y
        this->H(0,2) = 0.0;     // ∂d1/∂θ = 0
        
        this->H(1,0) = dx2/d2;  // ∂d2/∂x
        this->H(1,1) = dy2/d2;  // ∂d2/∂y  
        this->H(1,2) = 0.0;     // ∂d2/∂θ = 0
        
        this->V.setIdentity();
    }
    
private:
    Kalman::Vector<T, 2> l1, l2; // 两个landmark的位置
};

数学原理：测量模型基于距离测量：

$d_1 = \sqrt{(x - x_{l1})^2 + (y - y_{l1})^2}$
$d_2 = \sqrt{(x - x_{l2})^2 + (y - y_{l2})^2}$

雅可比矩阵 $H$ 包含测量对状态的偏导数：
$\begin{bmatrix} \frac{x-x_{l1}}{d_1} & \frac{y-y_{l1}}{d_1} & 0 \\ \frac{x-x_{l2}}{d_2} & \frac{y-y_{l2}}{d_2} & 0 \end{bmatrix}$

4.5 主函数流程说明

int main()
{
    // 1. 初始化滤波器参数
    T systemNoise = 0.1;        // 系统噪声
    T orientationNoise = 0.025;  // 方向测量噪声  
    T distanceNoise = 0.25;     // 距离测量噪声
    
    // 2. 创建系统模型和测量模型
    SystemModel<T> sysModel;
    PositionMeasurementModel<T> posModel;
    
    // 3. 设置噪声协方差矩阵
    sysModel.setCovariance(systemNoise);
    posModel.setCovariance(distanceNoise);
    
    // 4. 创建扩展卡尔曼滤波器
    ExtendedKalmanFilter<State> ekf;
    
    // 5. 初始化状态和滤波器
    State initialState;
    initialState.x() = 0.0;
    initialState.y() = 0.0; 
    initialState.theta() = 0.0;
    ekf.init(initialState);
    
    // 6. 仿真循环（100步）
    for(int i = 0; i < 100; ++i)
    {
        // 6.1 生成控制输入（速度和角速度）
        Control u;
        u.v() = 1.0 + 0.1 * std::sin(i * 0.1);
        u.dtheta() = 0.05 * std::cos(i * 0.05);
        
        // 6.2 预测步骤：使用系统模型和控制输入
        State x_pred = ekf.predict(sysModel, u);
        
        // 6.3 生成真实测量值（添加噪声）
        PositionMeasurement z_true = posModel.h(groundTruthState);
        PositionMeasurement z_noisy = z_true;
        z_noisy.d1() += distanceNoise * normal_distribution(generator);
        z_noisy.d2() += distanceNoise * normal_distribution(generator);
        
        // 6.4 更新步骤：使用测量模型和测量值
        State x_est = ekf.update(posModel, z_noisy);
        
        // 6.5 输出结果
        std::cout << i << "," << x_pred.x() << "," << x_pred.y() << ","
                  << x_est.x() << "," << x_est.y() << std::endl;
    }
    
    return 0;
}

主函数执行流程：

参数初始化：设置系统噪声和测量噪声参数
模型创建：实例化系统模型和测量模型
噪声配置：设置模型的协方差矩阵
滤波器初始化：创建EKF并设置初始状态
仿真循环：执行100次时间步
预测-更新循环：每个时间步执行预测和更新操作
结果输出：记录预测值和估计值用于分析

5. Eigen数学库在Kalman滤波中的关键应用

5.1 矩阵运算的数学原理

在卡尔曼滤波中，Eigen库提供了高效的矩阵运算支持：

矩阵乘法运算：

// 协方差预测：P = F * P * F^T + Q
P = (s.F * P * s.F.transpose()) + (s.W * s.getCovariance() * s.W.transpose());

数学原理：协方差预测公式 $P_{k|k-1} = FP_{k-1|k-1}F^T + Q$ 涉及矩阵乘法和转置运算，Eigen的表达式模板技术可以在编译时优化计算顺序，避免中间矩阵的创建。

矩阵求逆运算：

// 卡尔曼增益计算：K = P * H^T * (H * P * H^T + R)^(-1)
KalmanGain<Measurement> K = P * m.H.transpose() * S.inverse();

数学原理：卡尔曼增益计算需要求解线性方程组 $HPH^T + R)K = PH^T$ ，Eigen使用LU分解或Cholesky分解等数值稳定方法进行矩阵求逆。

5.2 状态向量运算

状态更新运算：

// 状态更新：x = x + K * (z - h(x))
x += K * (z - m.h(x));

数学原理：状态更新涉及向量加法和矩阵-向量乘法，Eigen的向量化指令（如SSE、AVX）可以显著加速这些运算。

5.3 协方差矩阵运算

协方差更新运算：

// 协方差更新：P = P - K * H * P
P -= K * m.H * P;

数学原理：Joseph形式协方差更新 $P_{k|k} = (I-KH)P_{k|k-1}(I-KH)^T + KRK^T$ 可以保证数值稳定性，Eigen支持这种复杂的矩阵运算。

5.4 矩阵模板特化

Kalman库中的矩阵定义：

// 状态向量的协方差矩阵
template<typename State>
using Covariance = Eigen::Matrix<typename State::Scalar, State::size, State::size>;

// 过程噪声协方差矩阵  
template<typename State>
using ProcessNoise = Eigen::Matrix<typename State::Scalar, State::size, State::size>;

// 测量噪声协方差矩阵
template<typename Measurement>  
using MeasurementNoise = Eigen::Matrix<typename Measurement::Scalar, Measurement::size, Measurement::size>;

5.5 高效的矩阵运算

预测步骤的矩阵运算：

// 协方差预测：P = FPFᵀ + Q
Covariance<State> P_pred = F * P * F.transpose() + Q;

// 使用Eigen的优化算法，避免临时矩阵创建
P_pred = F * P;
P_pred = P_pred * F.transpose(); 
P_pred += Q;

更新步骤的矩阵运算：

// 卡尔曼增益计算：K = PHᵀ(HPHᵀ + R)⁻¹
Matrix<T, State::size, Measurement::size> K;
Matrix<T, Measurement::size, Measurement::size> S = H * P_pred * H.transpose() + R;
K = P_pred * H.transpose() * S.inverse();  // 使用Eigen的高效求逆

5.6 数值稳定性保障

使用Eigen的分解算法：

// 对于病态矩阵，使用更稳定的分解方法
Eigen::LDLT<Matrix<T, Measurement::size, Measurement::size>> ldlt(S);
if(ldlt.info() != Eigen::Success) {
    // 处理数值问题
    Eigen::ColPivHouseholderQR<Matrix<T, Measurement::size, Measurement::size>> qr(S);
    K = P_pred * H.transpose() * qr.solve(Matrix<T, Measurement::size, Measurement::size>::Identity());
} else {
    K = P_pred * H.transpose() * ldlt.solve(Matrix<T, Measurement::size, Measurement::size>::Identity());
}

6. 工程实践与性能优化

6.1 内存管理优化

静态内存分配：

// 使用固定大小的矩阵避免动态内存分配
typedef Kalman::Vector<float, 3> State;  // 编译时确定大小
typedef Eigen::Matrix<float, 3, 3> Covariance;  // 静态分配

内存池技术：

// 为频繁使用的矩阵预分配内存
class MatrixPool {
    std::vector<Covariance<State>> covariancePool;
    std::vector<ProcessNoise<State>> noisePool;
public:
    Covariance<State>& getCovariance() {
        if(covariancePool.empty()) {
            return *new Covariance<State>();
        }
        auto& mat = covariancePool.back();
        covariancePool.pop_back();
        return mat;
    }
};

6.2 计算性能优化

表达式模板优化：

// Eigen的表达式模板避免不必要的中间矩阵
Covariance<State> P_new = (Covariance<State>::Identity() - K * H) * P_pred;
// 实际计算时，Eigen会优化为：P_new = P_pred - K * (H * P_pred)

并行化计算：

// 利用Eigen的向量化特性
#ifdef EIGEN_VECTORIZE
// 启用SIMD指令集加速
#pragma omp parallel for
for(int i = 0; i < State::size; ++i) {
    // 并行处理状态分量
}
#endif

7. 调试与验证方法论

7.1 一致性检验

归一化创新平方（NIS）检验：

T calculateNIS(const Measurement& z, const Measurement& z_pred, 
               const Matrix<T, Measurement::size, Measurement::size>& S) {
    Measurement innovation = z - z_pred;
    T nis = innovation.transpose() * S.inverse() * innovation;
    return nis;
}

// NIS应该符合卡方分布，用于验证滤波器一致性
bool checkConsistency(T nis, T chiSquareThreshold) {
    return nis < chiSquareThreshold;
}

7.2 蒙特卡洛仿真验证

class MonteCarloValidation {
    std::vector<State> trueStates;
    std::vector<State> estimatedStates;
    
public:
    void runSimulation(size_t numRuns, size_t numSteps) {
        for(size_t run = 0; run < numRuns; ++run) {
            // 运行完整的滤波过程
            auto results = runSingleSimulation(numSteps);
            trueStates.push_back(results.trueState);
            estimatedStates.push_back(results.estimatedState);
        }
        
        // 计算统计性能指标
        calculateRMSE();
        calculateNEES();  // 归一化估计误差平方
    }
};