[Kalman滤波]算法的基本推导证明（含C 与MATLAB实现）

前言

卡尔曼滤波器（Kalman Filter）是一种递归的、最优的状态估计算法，用于估计动态系统的状态。它基于系统的数学模型和测量数据，通过融合预测和测量信息，提供对系统状态的最佳估计。

卡尔曼滤波器的基本原理是通过递归地进行两个步骤：预测步骤（Prediction）和更新步骤（Update）。在预测步骤中，根据系统的数学模型和当前状态的估计，计算出系统在下一个时间步的预测状态和协方差。在更新步骤中，将预测的状态与实际的测量值进行比较，根据两者之间的差异以及测量误差的协方差，更新状态的估计和协方差。

卡尔曼滤波器的核心思想是在预测和更新过程中，根据测量值和模型预测之间的权衡，进行加权平均来得到最佳的状态估计。通过动态调整权重，卡尔曼滤波器能够自适应地处理系统模型不完全和测量误差的情况，从而提供对系统状态的准确估计。

卡尔曼滤波器广泛应用于各种领域，包括导航系统、控制系统、机器人技术、信号处理和金融等。它具有高效、精确和适应性强的特点，在许多实时应用中发挥着重要作用。

先上结论，便于不需推导证明的同学快速查阅

实际模型与卡尔曼五大方程

离散时间卡尔曼估计器计算过程的基本步骤：

1. 使用 、和计算 .

2. 使用（在步骤 1 中计算）、 和 计算 。

3. 使用 （在步骤 2 中计算）和（来自步骤 1）计算 。

4. 使用的计算值（来自步骤 2）、给定的初始估计 和输入数据 ，递归地计算 的连续值。

滤波器与实际系统的关系：

卡尔曼滤波器的计算时序：

卡尔曼滤波推导（线性离散形式）

推导过程中参数、矩阵、假设说明：

推导过程

基本模型与估计方法

对于离散线性系统（系统1式）

PS:若系统形式写为也可使用卡尔曼滤波，计算误差过程中会消去。

是过程噪声。

假设在时间进行了测量，并且它提供的信息将用于更新随机系统在时间 的状态 的估计。假设测量与状态线性相关，方程形式为（系统2式）：

其中 是测量灵敏度矩阵， 是测量噪声。都是高斯白噪声

设 的状态 的最佳估计为，求取过程应当包含 的先验估计（估计值）  和观测值，并结合两者的权重和，由式（3）所求（卡尔曼2-1式）：

其中 是 的先验估计（估计值）， 是估计的后验值（最佳估计值），和为卡尔曼增益。

先验估计（估计值）按照最简便的方法，直接依据系统状态方程（系统1式）计算，得到（状态估计方程，卡尔曼1式）：

求卡尔曼增益

接下来要去求解卡尔曼增益和.

按照什么原则选取卡尔曼增益和？可以采用最优化方法中的正交性原则（orthogonality principle），这个正交条件可以写成下面的形式（这里不推导正交条件了，有证明需求的同学可参考北航王可东教授的《Kalman滤波基础及MALTAB仿真》中5.2.4节）：

将(1)、（2）、（3）代入（4）

由高斯白噪声的性质及二者互不相关可知：

考虑式（4）的之前的时间点

（5）可以化简为（7）：

所以

那么（3）式中所需求的两个卡尔曼增益和的关系已经找到，知道一个可求第二个，（3）式可写为（状态（最优估计）更新方程，卡尔曼2-2式）：

定义后验值（最佳估计值）与实际值的偏差（10）和先验值（估计值）与实际值的偏差（11）：

那么依据定义测量值与实际测量值的偏差为：

用（4）减去（12）可得

替换其中的，得到

根据白噪声的性质，去掉等于零的部分，最后化简为：

根据定义，先验协方差（更新前的误差协方差矩阵）为

（16）代入（15）可得

从（17）导出（卡尔曼增益，卡尔曼3式）：

更新误差协方差矩阵

可以推导出后验协方差（更新后的误差协方差矩阵）的类似公式，其定义为

对（9）的两边同时减去

(18）代入（20）可得

将公式(18)中的 代入，可以得到以下形式（误差协方差更新方程，卡尔曼4式）：

接下来需要计算先验协方差（更新前的误差协方差矩阵）

最终可得（误差协方差估计方程，卡尔曼5式）

（25）它给出了估计不确定性协方差矩阵的先验值作为先前后验值的函数。

最终可总结为文章开头处的结论。

Kalman滤波-C++实现

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
/*
定义了一个名为KalmanFilter的结构体，它包含了卡尔曼滤波器的相关参数和状态。
state是状态向量，covariance是估计误差协方差矩阵，transitionMatrix是状态转移矩阵，
controlMatrix是控制输入矩阵，observationMatrix是观测矩阵，processNoiseCov是过程噪声协方差矩阵，
measurementNoiseCov是观测噪声协方差矩阵。
*/
struct KalmanFilter {
    VectorXd state;            // 状态向量
    MatrixXd covariance;       // 估计误差协方差矩阵
    MatrixXd transitionMatrix; // 状态转移矩阵
    MatrixXd controlMatrix;    // 控制输入矩阵
    MatrixXd observationMatrix; // 观测矩阵
    MatrixXd processNoiseCov;  // 过程噪声协方差矩阵
    MatrixXd measurementNoiseCov; // 观测噪声协方差矩阵

    /*
    定义了一个predict函数，用于执行卡尔曼滤波的预测步骤。函数接受一个控制输入向量controlInput作为参数。
    在预测步骤中，根据状态转移矩阵和控制输入矩阵，更新状态向量state和估计误差协方差矩阵covariance。
    */
    void predict(const VectorXd& controlInput) {
        state = transitionMatrix * state + controlMatrix * controlInput;
        covariance = transitionMatrix * covariance * transitionMatrix.transpose() + processNoiseCov;
    }
    /*
    定义了一个update函数，用于执行卡尔曼滤波的更新步骤。函数接受一个观测向量measurement作为参数。
    在更新步骤中，根据观测矩阵，计算创新协方差矩阵innovationCov，然后计算卡尔曼增益矩阵kalmanGain。
    最后，根据观测数据和卡尔曼增益，更新状态向量state和估计误差协方差矩阵covariance。
    */

    void update(const VectorXd& measurement) {
        MatrixXd innovationCov = observationMatrix * covariance * observationMatrix.transpose() + measurementNoiseCov;
        MatrixXd kalmanGain = covariance * observationMatrix.transpose() * innovationCov.inverse();

        state = state + kalmanGain * (measurement - observationMatrix * state);
        covariance = (MatrixXd::Identity(covariance.rows(), covariance.cols()) - kalmanGain * observationMatrix) * covariance;
    }
};

int main() {
    // 定义系统模型和观测模型的参数
    MatrixXd A(2, 2);
    A << 1, 1, 0, 1;
    MatrixXd B(2, 1);
    B << 0.5, 1;
    MatrixXd C(1, 2);
    C << 1, 0;
    MatrixXd Q(2, 2);
    Q << 0.1, 0, 0, 0.1;
    MatrixXd R(1, 1);
    R << 1;

    /*
    在main函数中，定义了系统模型和观测模型的参数矩阵：A是状态转移矩阵，B是控制输入矩阵，C是观测矩阵，Q是过程噪声协方差矩阵，R是观测噪声协方差矩阵。
    然后，创建了一个KalmanFilter类型的对象kf，并为其成员变量赋值。初始化状态向量state为零向量，
    初始化估计误差协方差矩阵covariance为2x2的单位矩阵，其他参数矩阵使用之前定义的矩阵。
    */

    // 初始化卡尔曼滤波器
    KalmanFilter kf;
    kf.state = VectorXd(2);
    kf.state << 0, 0;
    kf.covariance = MatrixXd::Identity(2, 2);
    kf.transitionMatrix = A;
    kf.controlMatrix = B;
    kf.observationMatrix = C;
    kf.processNoiseCov = Q;
    kf.measurementNoiseCov = R;

    /*
    接下来，定义了一个长度为10的观测向量measurements，并赋予一些示例观测值。
    然后，使用一个循环遍历观测向量中的每个观测值。 在每个时间步骤中，我们先调用kf.predict函数进行预测，将控制输入向量设置为零向量。
    然后，调用kf.update函数进行更新，传入当前的观测值作为参数。
    最后，使用std::cout输出当前时间步骤的估计状态，使用kf.state.transpose()将状态向量转置为行向量输出。
    */

    // 模拟数据
    VectorXd measurements(10);
    measurements << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;

    // 执行卡尔曼滤波
    for (int i = 0; i < measurements.size(); ++i) {
        kf.predict(VectorXd::Zero(1));  // 在此示例中，没有控制输入，所以将控制输入设置为零向量
        kf.update(VectorXd::Constant(1, measurements(i)));  // 使用当前观测数据进行更新
        std::cout << 'Estimated state at time step '<< i + 1 << ': ' << kf.state.transpose() << std::endl;
    }

    return 0;
}


Kalman滤波-MATLAB实现

% 定义系统模型和观测模型的参数
A = [1, 1; 0, 1];
B = [0.5; 1];
C = [1, 0];
Q = [0.1, 0; 0, 0.1];
R = 1;

% 初始化卡尔曼滤波器
initialState = [0; 0];
initialCovariance = [1, 0; 0, 1];
state = initialState;
covariance = initialCovariance;

% 模拟数据
measurements = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10];

% 执行卡尔曼滤波
for i = 1:length(measurements)
    % 预测步骤
    predictedState = A * state + B * 0;  % 在此示例中，没有控制输入，所以将控制输入设置为零向量
    predictedCovariance = A * covariance * A' + Q;

    % 更新步骤
    innovationCovariance = C * predictedCovariance * C' + R;
    kalmanGain = predictedCovariance * C' / innovationCovariance;

    state = predictedState + kalmanGain * (measurements(i) - C * predictedState);
    covariance = (eye(size(state, 1)) - kalmanGain * C) * predictedCovariance;

    disp(['Estimated state at time step ', num2str(i), ':']);
    disp(state');
end

卡尔曼滤波的常见变体

1. 扩展卡尔曼滤波器（Extended Kalman Filter，EKF）：适用于非线性系统的卡尔曼滤波器扩展。EKF通过对非线性系统模型进行线性化，利用雅可比矩阵来近似系统模型，并在预测和更新步骤中使用这些线性化模型进行状态估计。
2. 无迹卡尔曼滤波器（Unscented Kalman Filter，UKF）：用于非线性系统的改进型卡尔曼滤波器。UKF通过使用一组无迹变换（Unscented Transform）选择的采样点，以更准确地捕捉非线性系统的特性。这些采样点通过非线性变换来代表状态的均值和协方差。
3. 滑动窗口卡尔曼滤波（Sliding Window Kalman Filter，SWKF）：用于处理时变系统或流数据。SWKF通过滑动窗口技术，对一段时间内的数据进行处理，以获取更准确的状态估计。
4. 自适应卡尔曼滤波（Adaptive Kalman Filter，AKF）：适用于系统噪声方差或测量噪声方差随时间变化的情况。AKF通过对方差进行实时调整，以适应系统的变化。

参考文献：KALMAN FILTERING Theory and Practice Using MATLABw Third Edition