滤波算法（一）：卡尔曼滤波算法

卡尔曼滤波是什么

卡尔曼滤波算法是一种通过对测量数据进行加权平均处理来估计未知变量的算法，它是一种基于状态空间模型的最优估计算法，尤其适用于线性系统。它通过利用系统的动态模型和观测模型，将系统的状态估计和观测值进行融合，从而得到更准确的估计值。

卡尔曼滤波系统的两个方程

什么是状态方程？观测方程？【现代控制理论概念】

状态方程是描述某个物理系统状态随时间变化的数学表达式。它通常是以微分方程或差分方程的形式表示，并描述系统的状态变量随时间的变化。状态方程可以用来预测系统未来的状态或根据已知的状态预测系统的过去状态。
观测方程是用于描述一个物理系统的状态变量与实际可观测量之间关系的数学表达式。它通常是以代数方程或非线性方程的形式表示，将系统的状态变量转换为实际测量值。观测方程是用来确定系统的状态变量，常用于控制系统、信号处理或时间序列分析等领域。

卡尔曼滤波系统的状态方程

假设系统状态Xk 为N次仪器测量结果的平均值（如下图所示），可以看出系统状态Xk与Xk-1的关系，继而推出卡尔曼滤波器状态方程。

卡尔曼滤波系统的观测方程

卡尔曼滤波的五大公式

预测公式

先验估计

误差协方差

更新公式（校正公式）

计算卡尔曼增益

修正估计

zk是测量值

更新误差协方差

上述公式，注意搞清楚各个变量概念

卡尔曼滤波应用

卡尔曼滤波算法代码

卡尔曼滤波结构体

typedef struct __KalmanTypeDef {
    float x; // 最优输出
    float A; // 状态转移矩阵
    float H; // 状态检测转换矩阵
    float q; // 噪声
    float r;  // 测量噪音协方差
    float p; // 最优输出方差
    float gain;// kg 卡尔曼增益
} KalmanTypeDef;

初始化和更新

void KalmanFilter_Init(KalmanTypeDef *cfg) {
    cfg->x = 0;
    cfg->p = 5;
    cfg->A = 1;
    cfg->H = 1;
    cfg->q = 0.25;
    cfg->r = 1;
}

float KalmanFilter_Update(KalmanTypeDef *cfg, float measure) {
    cfg->x = cfg->A * cfg->x;  //计算估计值
    cfg->p = cfg->A * cfg->A * cfg->p + cfg->q;//计算估计值方差

    cfg->gain = cfg->p * cfg->H / (cfg->p * cfg->H * cfg->H + cfg->r); //计算卡尔曼增益
    cfg->x = cfg->x + cfg->gain * (measure - cfg->H * cfg->x); //计算最优输出
    cfg->p = (1 - cfg->gain * cfg->H) * cfg->p; // 计算最优输出方差

    return cfg->x;
}

算法调试

卡尔曼滤波器设计思路

1. 选择状态量、观测量，构建状态方程、观测方程
2. 初始化参数，带入五大公式
3. 调试参数

调试参数方法

1. P0 和 X0 的取值
   一般 X0 = 0，P0取较小的数（一般为1，不可为0），经过几轮迭代会趋于稳定值

2. Q 和 R的取值
   

当我们更信任观测值时，那么应该让卡尔曼增益K增大；从K的公式中可以看出，R越小K越大，Q越大K越大；
当我们更信任模型估计值时，那么应该让卡尔曼增益K减小；从K的公式中可以看出，R越大K越小，Q越小K越小

参考资料

详解卡尔曼滤波原理
卡尔曼滤波算法详细推导
从放弃到精通！卡尔曼滤波从理论到实践~