卡尔曼滤波-应用白话

或者不懂原理，可以通过简单示例，理解应用。

void kalmanFilter(double *measurements, double *estimates, int numMeasurements, double processNoise, double measurementNoise)

5个参数：

• 传入测量数据：输入数据
• 估计值数组：结果
• 测量数据的数量：输入数据数量
• 过程噪声：调节参数
• 测量噪声：调节参数

过程噪声和测量噪声是卡尔曼滤波器的重要调节参数，用于优化滤波器的性能。它们的设置对滤波器的输出影响显著，通常需要根据测量对象的特性进行调整。

1. 过程噪声（Process Noise）

通俗解释：
过程噪声反映的是你对系统本身变化的不可预测性有多大的信心。如果系统变化很快、很随机，那么过程噪声应该设置得大一些；如果系统变化很慢或者你很确定系统的变化规律，那么过程噪声就应该小一些。

• 示例1：无人驾驶汽车的位置预测

假设你在跟踪一辆无人驾驶汽车的位置，汽车的运动相对平缓且可以预测。此时你可以把过程噪声设置得比较小，因为汽车不会突然瞬移或做出非常剧烈的变化。

过程噪声小：你相信汽车的运动是可以预测的，卡尔曼滤波器会更依赖之前的预测值。

但是，如果这辆汽车可能会突然加速、急刹车，或者遇到突发情况（比如躲避障碍物），那么你对汽车的未来位置预测就会变得不确定。这时你需要把过程噪声设置得大一些。

过程噪声大：你不太确定汽车的未来位置，卡尔曼滤波器会更依赖当前的测量值。

• 示例2：股票价格预测

如果你在用卡尔曼滤波器预测股票价格，由于股票价格变化非常快且不可预测，过程噪声应该设置得大一些，因为价格可能会突然大幅度波动。

过程噪声大：股票价格变化非常随机，卡尔曼滤波器不会过度依赖之前的预测。

2. 测量噪声（Measurement Noise）

通俗解释：
测量噪声反映的是你对传感器或测量工具的信任程度。如果你的传感器非常精准（例如高精度GPS），那么测量噪声可以设置得小一些；如果传感器质量不高或者容易受到外界干扰（例如廉价的温度计），那么测量噪声就应该大一些。

示例1：温度计测量

如果你使用的是高精度的数字温度计，测量噪声可以设置得比较小，因为温度计的读数比较可靠，卡尔曼滤波器可以更多地依赖这些读数。

测量噪声小：传感器很精确，卡尔曼滤波器会更相信测量值。

如果你使用的是一个非常便宜且不稳定的温度计，读数经常跳变，那么你需要把测量噪声设置得大一些，因为你不太确定温度计的读数是否准确。

测量噪声大：传感器不可靠，卡尔曼滤波器不会完全依赖测量值，而是会结合之前的预测值进行综合判断。

示例2：GPS定位

如果你使用的是高精度的GPS设备，测量噪声可以设置得较小，因为GPS的定位相对准确。

测量噪声小：GPS数据可靠，卡尔曼滤波器会更依赖GPS的测量值。

如果你使用的是手机上的普通GPS，定位精度不高，可能有几十米的误差，那么测量噪声就需要设置得大一些。

测量噪声大：GPS数据不可靠，卡尔曼滤波器会更多地参考之前的预测，而不是完全依赖当前的GPS数据。

如何调节这两个参数？

1. 过程噪声调节

场景：系统变化慢且可预测（如缓慢移动的汽车，稳定的温度）
过程噪声小：例如 processNoise = 0.1
场景：系统变化快且不可预测（如急刹车、股票价格）
过程噪声大：例如 processNoise = 1.0

2. 测量噪声调节

场景：传感器精度高（如高精度GPS，数字温度计）
测量噪声小：例如 measurementNoise = 0.1
场景：传感器精度低（如手机GPS，便宜的温度计）
测量噪声大：例如 measurementNoise = 1.0

挑战自己，看看看看看看不懂的原理

卡尔曼滤波的简单实现(Matlab & OC)

#include <stdio.h>
#include <stdlib.h>

// 卡尔曼滤波函数
void kalmanFilter(double *measurements, double *estimates, int numMeasurements, double processNoise, double measurementNoise) {
    int i;
    double estimate, priorEstimate, priorError, errorCovariance;
    double kalmanGain, temp;
    
    // 初始化状态估计和协方差
    estimate = 0.0;
    priorEstimate = 0.0;
    priorError = 1.0; // 初始估计误差设为1.0
    errorCovariance = priorError * priorError;
    
    for (i = 0; i < numMeasurements; i++) {
        // 计算卡尔曼增益
        kalmanGain = errorCovariance / (errorCovariance + measurementNoise);
        // 更新状态估计
        estimate = priorEstimate + kalmanGain * (measurements[i] - priorEstimate);
        // 更新协方差
        errorCovariance = (1 - kalmanGain) * errorCovariance;
        // 更新先验估计和误差
        priorEstimate = estimate;
        priorError = errorCovariance;
    }
    // 将最终估计存储在estimates数组中
    estimates[0] = estimate;
}

// 主函数，演示卡尔曼滤波
int main() {
    double measurements[] = {1.0, 2.0, 3.0, 2.5, 3.5, 2.0, 3.0, 4.0, 5.0};
    double estimates[1]; // 用于存储最终估计的数组
    int numMeasurements = sizeof(measurements) / sizeof(measurements[0]);
    double processNoise = 0.1; // 过程噪声
    double measurementNoise = 0.2; // 测量噪声
    kalmanFilter(measurements, estimates, numMeasurements, processNoise, measurementNoise);
    printf("Estimated value: %f\n", estimates[0]);
    return 0;
}