【控制算法笔记】卡尔曼滤波（一）——基本概念和一维卡尔曼估计实现（python,C++）

本文是个人学习笔记，包含个人理解，如有错误欢迎指正。

前言–关于Kalman Filter

在工程实践中卡尔曼滤波器的应用场景非常丰富，尤其是针对需要大量连续数据处理的自动驾驶和工业现场控制场景中，几乎离不开卡尔曼滤波的踪迹。
在多年前刚接触到单片机的时候对各种算法还不是很了解，当时因为一些比赛需要使用到IMU做角度闭环控制，第一次接触到了卡尔曼滤波器。记得印象中当时使用的是MPU6050计算四元数角度，卡尔曼滤波器可以很好的规避传感器在数据读取的过程中随机的噪声信号，保证一定时间段内读取的数据的稳定性。
那么卡尔慢滤波器是如何起作用的？个人感觉这更像是一个符合概率分布条件下的数值估计器，当保证输入数据附加的噪声能满足先验的概率分布，算法就可以实时计算当前输入数据最符合概率分布的结果并输出。为了实现这一效果，卡尔曼滤波器需要获得输入信号噪声的分布情况以及传感器采集数据的误差范围，同时还需要给系统附加一个计算初值，这些参数都会影响到估计的准确性。在实际的生产应用中有时这些噪声和误差的分布难以做到精准建模来获得数据，但是得益于卡尔慢滤波器中的卡尔慢增益，算法其实可以在一定范围内较好的适应噪声情况，只需要一个近似的估计参数，算法会尽最大可能保证估计值的准确性。也正是应为这个原因才使得卡尔慢滤波器及各种扩展的优化算法得到广泛应用。
对于卡尔慢滤波的估计效果，网上有不少的应用演示是使用卡尔慢滤波相关的算法对某个对象的数据进行未来估计，其实这个说法并不靠谱，Kalman Filter（KF）实质只是一个滤波器，是一个当前实际数据的估计器，是站在过去一段时间的数据上估计当前实际值，它并不能预测未来一段时间的数据。这个概念一定要分清，如果说要预测未来的数据，那就得尝试其它方法，要么对数据进行建模得到对象的数学模型，要么尝试一些机器学习的方式来生成对象的网络模型等。在这样的基础上才可以进一步使用KF去估计模型中的参数等，达到使用KF来辅助预测的效果。

最优递归数字信号处理

首先需要认识到在工程物理的测量中信号存在一定程度的不确定性。因为不存在完美的数学建模，实际物理系统也始终存在扰动，并且测量工具本身也会存在测量误差。

统计样本和误差数据

所以定义以下两种误差形式：测量误差，读取误差（估计误差）
当在一个足够大的观测样本中，可以使用样本均值和样本方差来表征采样对象的统计分布规律。对一组样本有均值：$$\widehat{x_k}=\frac{1}{k}(z_1+z_2+...+z_n)=\frac{1}{k}\sum _{i=1}^k{z}_i$$ 拆开均值后凑‘1’可得：$$\widehat{x_k}=\frac{1}{k}\sum _{i=1}^k{z}_i=\frac{1}{k}\sum _{i=1}^{k-1}{z}_i+\frac{1}{k}{z}_k$$ $$\Rightarrow \widehat{x_k}=\frac{1}{k}\frac{k-1}{k-1}\sum _{i=1}^{k-1}{z}_i+\frac{1}{k}{z}_k=\frac{k-1}{k}\widehat{x_{k-1}}+\frac{1}{k}{z}_k$$ $$\Rightarrow \widehat{x_k}=\frac{k-1}{k}{\hat{x}}_{k-1}+\frac{1}{k}{z}_k={\hat{x}}_{k-1}+\frac{1}{k}(z_k-{\hat{x}}_{k-1})$$ 最后可以得到采样数据的最后一个估计值等于上一次的估计值加上当前测量值与上一次估计值之差。
在上面的最后一个公式中：$$\widehat{x_k}={\hat{x}}_{k-1}+\frac{1}{k}(z_k-{\hat{x}}_{k-1})$$ 不妨设后一项的系数 $\frac{1}{k}$为 $K_k$ 称之为卡尔慢增益。并且卡尔慢增益的值为：$$K_k=\frac{e_{ES{T}_{K-1}}}{e_{ES{T}_{K-1}}-e_{ME{A}_K}}$$ 其中 $e_{EST}$为估计误差 $e_{MEA}$为测量误差。
对卡尔慢增益（Kalman Gain，KG）很容易可以看出：

• 当上一个时刻的估计误差远远大于当前时刻的测量测量误差是KG趋于1，当前时刻的估计值就等于当前时刻的测量值
• 当上一个时刻的估计误差远远小于当前时刻的测量误差时，KG趋于0，当前时刻的估计值就等于上一个时刻的估计值

在实际处理测量数据时往往分三步实现：
Step1: 计算当前时刻的KG值$$KG=K_k=\frac{e_{ES{T}_{K-1}}}{e_{ES{T}_{K-1}}-e_{ME{A}_K}}$$
Step2: 计算当前时刻的估计值 $$\widehat{x_k}={\hat{x}}_{k-1}+K_k(z_k-{\hat{x}}_{k-1})$$
Step3: 更新当前时刻的估计误差 $$e_{ES{T}_k}=（1-K_k）e_{ES{T}_{k-1}}$$

通过不断递归循环最终可以估计出概率上较为准确的估计值。

程序示例

使用python实现简单的卡尔慢估计

import numpy as np
import matplotlib.pyplot as plt

# 生成一维正态分布的随机数组（测量值）
mean_value = 100
std_dev_measurement = 5

np.random.seed(42)  # 设置随机数种子，保证重复运行的结果不变
# 生成一维正态分布的随机数组
random_array = np.random.normal(mean_value, std_dev_measurement, size=80)

# 初始化卡尔曼滤波器参数
state_estimate = mean_value  # 初始状态估计
state_covariance = 2  # 初始状态协方差
process_noise = 2  # 系统演化的噪声
measurement_noise = std_dev_measurement**2  # 测量误差的噪声
predicted_state = 80 # 预测初值

# 存储估计值
estimated_states = []

# 卡尔曼滤波过程
for measurement in random_array:
    # 1、计算卡尔曼增益
    predicted_covariance = state_covariance + process_noise     # 赋值估计误差，估计误差同时加上了一个系统的动态噪声
    kalman_gain = predicted_covariance / (predicted_covariance + measurement_noise)     # 使用估计误差核测量误差求取卡尔曼增益

    # 2、计算当前时刻的估计值
    state_estimate = predicted_state + kalman_gain * (measurement - predicted_state)    # 计算当前时刻的预测值

    # 3、更新当前时刻的估计误差
    state_covariance = (1 - kalman_gain) * predicted_covariance # 更新估计误差

    estimated_states.append(state_estimate)     # 保存当前时刻的估计值
    predicted_state = state_estimate  # 保存上一个时刻的预测值

# 绘制结果
plt.plot(random_array, label='observed value')
plt.plot(estimated_states, label='Kalman estimated value')
plt.plot([0, 80], [100, 100], label='true value', color='red')
plt.legend()
plt.title('Kalman Filter')
plt.show()

在上面的程序中通过随机生成了一组均值为100，方差为5的数据，并且给测量数据附加了大小为12（5*2+2）的扰动数据。令卡卡尔曼滤波器的初始参数为：卡尔曼增益为4，初始估计值为80。根据结果可以看出最后随着估计次数的增加，估计的精度不断提高。

使用C++/C实现卡尔曼滤波估计

通常卡尔曼滤波需要设计到很多嵌入式系统中，下面同时给出了cpp形式的代码，主要的计算部分将存储数据的vector修改为数组就是c兼容的代码。注意由于c++没有python那样便利的可视化和调包机制，这里画图软件使用GNU Plot，可以自行搜索安装该程序。

#include <iostream>
#include <vector>

#include<stdio.h>
#include<string.h>
#include <fstream>
#include <sstream>
#include <cmath>
int main() {
    const int size = 80;                    // 生成的数据长度
    double mean_value = 100.0;              // 待生成的随机数的均值
    double std_dev_measurement = 5.0;       // 随机数的方差
    double process_noise = 2.0;             // 系统的扰动误差
    double predicted_state = 80.0;          // 预测初值k-1步
    double state_estimate = mean_value;     // 预测初值k步
    double state_covariance = 2.0;          // 初始的估计误差
    double measurement_noise = std_dev_measurement * std_dev_measurement;   // 测量误差

    // 设置随机数种子，保证重复运行的结果不变
    std::srand(42);
    // 生成一维正态分布的随机数组
    std::vector<double> random_array;
    // Box-Muller算法利用均匀分布产生高斯分布随机数
    for (int i = 0; i < 80; ++i) {
        double U1 = rand() * 1.0f / RAND_MAX; // 0~1均匀分布
        double U2 = rand() * 1.0f / RAND_MAX; // 0~1均匀分布
        double Z = sqrt(-2 * log(U1)) * cos(2 * 3.1415 * U2);// 均值为0，方差为1的正态分布
        double Y = 100 + 3 * Z; // 均值为100，方差为9的正态分布
        random_array.push_back(Y);
    }

    // 存储估计值
    std::vector<double> estimated_states;

    // 卡尔曼滤波过程
    for (const auto& measurement : random_array) {
        // 1、计算卡尔曼增益
        double predicted_covariance = state_covariance + process_noise;     // 计算估计误差
        double kalman_gain = predicted_covariance / (predicted_covariance + measurement_noise); // 计算卡尔曼增益

        // 2、计算当前时刻的估计值
        state_estimate = predicted_state + kalman_gain * (measurement - predicted_state);

        // 3、更新当前时刻的估计误差
        state_covariance = (1 - kalman_gain) * predicted_covariance;

        estimated_states.push_back(state_estimate);
        predicted_state = state_estimate;   // 保存当前时刻的估计值
    }

    // 输出当前估计值到文件
    std::ofstream dataFile("data.txt");
    // 输出估计值
    std::cout << "Estimated States:" << std::endl;
    int index = 0;
    for (const auto& estimate : estimated_states) {
        std::cout << estimate << " ";
        dataFile << index << " " << (double)estimate << " " << index << " "  << mean_value << " "<< index<<" "<< random_array[index] <<std::endl;
        index++;
    }
    std::cout << std::endl;
    dataFile.close();

    //  =======================    下面的代码都是为了画图     ======================
    // 创建一个GNU Plot进程
    FILE* gnuplotPipe = _popen("gnuplot -persist", "w");
    // 设置GNU Plot的属性
    if (gnuplotPipe) {
        fprintf(gnuplotPipe, "set size 1,1\n");
        fprintf(gnuplotPipe, "set title 'Kalman Filter'\n");
        fprintf(gnuplotPipe, "set xlabel 'Iteration'\n");
        fprintf(gnuplotPipe, "set ylabel 'Value'\n");

        // 发送GNUplot命令绘制动态图
        fprintf(gnuplotPipe, "plot 'data.txt' using 1:2 with lines linestyle 6 title 'Kalman estimated value', \
                          'data.txt' using 3:4 with lines linestyle 3 title 'true value', \
                          'data.txt' using 5:6 with lines linestyle 3 title 'observed value'\n");
        fflush(gnuplotPipe);
        fprintf(gnuplotPipe, "pause mouse\n");
        _pclose(gnuplotPipe);
    }
    else {
        std::cerr << "Error opening GNUplot pipe" << std::endl;
    }
    return 0;
}

通过可是化输出可知系统有良好的估计能力。
对于matlab程序可以参考上面的python代码，很容易可以改写出来相同效果程序。