2.一维卡尔曼滤波(动态模型)

原创已于 2025-04-17 11:13:12 修改 · 1.2k 阅读

30 ·

CC 4.0 BY-SA版权

文章标签：

#算法

于 2025-04-16 19:32:41 首次发布

算法专栏收录该内容

3 篇文章

订阅专栏

1. 使用场景

测量的是1维的数据,只有一个测量值(例如:电压,电流,温湿度采集)----采集的数据是随机的
和测量硬币直径不同-----硬币的直径是固定的

2. 流程图

在这里插入图片描述
下面是一维的卡尔曼滤波示例,由于一维的特殊性,有些变量是1或者没有,导致公式省略了好多,
建议你先去研究一下3.二维-卡尔曼滤波算法,弄明白了再回来看一维的,就简单明了了

1. 先验估计计算

📌 固定公式 :
$\displaystyle{{X}_{k}}=A⋅{{X}_{k-1}}+B⋅{{U}_{k}}+{{W}_{k}}$

• $\displaystyle {{X}_{k}} : \text{是状态向量}$
• $\displaystyle {A}=\left [{\begin{matrix}1\end{matrix}}\right ]: \text{是状态转移矩阵}$
• $\displaystyle {B}=\left [{\begin{matrix}0\end{matrix}}\right ]:是输入矩阵(没有)$
• $\displaystyle {{U}_{k}}:是控制输入向量(没有)$
• $\displaystyle {{W}_{k}}:是过程噪声向量(省略)$

📌 状态方程就可以写作：
$\displaystyle{{X}_{k}}=A⋅{{X}_{k-1}}$
$\displaystyle{{X}_{k}}={{X}_{k-1}}$ //代码1

2. 预测协方差矩阵

📌 固定公式 :
$\displaystyle{{P}_{k}}=A⋅{{P}_{k-1}}⋅{{A}^{T}}+Q={{P}_{k-1}}+Q$

• $\displaystyle {Q}:过程噪声协方差矩阵,需要自定义好$

static float Q = 0.01; // 代码01 过程噪声协方差矩阵

📌
$\displaystyle{{P}_{k}}={{P}_{k-1}}+Q$ //代码2

在这里插入图片描述

3.建立测量方程

📌 固定公式 :
$\displaystyle{{Z}_{k}}=H{{X}_{k}}+{{V}_{k}}$

📌
$\displaystyle{{Z}_{k}}=Temp (传感器获取到的温度) //代码3$

4.计算卡尔曼增益

📌 固定公式 :
$\displaystyle{{K}_{g(k)}}=\frac{{{P}_{k-1}}⋅{{H}^{T}}}{H{{P}_{k-1}}⋅{{H}^{T}}+R} = \frac{{{P}_{k-1}}}{{{P}_{k-1}}+R}$

• $\displaystyle {{P}_{k-1}}:是先验状态协方差({{P}_{k}}的初始值=1)$

• $\displaystyle {H}:是测量矩阵\left [{\begin{matrix}1\end{matrix}}\right ]$

• $\displaystyle {R}:是测量噪声的协方差,需要自定义好$

static float R = 0.05;  // 代码02 测量噪声的协方差

📌
$\displaystyle{{K}_{g(k)}}=\frac{{{P}_{k-1}}}{{{P}_{k-1}}+R}$ //代码4

R=0时,卡尔曼滤波的曲线就会和 测量值 $\displaystyle {{Z}_{k}}$ 重合
在这里插入图片描述

5.计算当前最优估计值

📌 固定公式 :
$\displaystyle{{X}_{k}}={{X}_{k-1}}+{{K}_{g(k)}}⋅({{Z}_{k}}-H⋅{{X}_{k-1}})$

H = 1,所以

📌
$\displaystyle{{X}_{k}}={{X}_{k-1}}+{{K}_{g(k)}}⋅({{Z}_{k}}-{{X}_{k-1}})$ //代码5

6.更新协方差矩阵

📌 固定公式 :
$\displaystyle{{P}_{k}}=[I-{{K}_{g(k)}}⋅H]⋅{{P}_{k-1}}$

$\displaystyle I$ :单位矩阵 $\displaystyle\left [{\begin{matrix}1\end{matrix}}\right ]$

📌
$\displaystyle{{P}_{k}}=[1-{{K}_{g(k)}}]⋅{{P}_{k-1}}$ //代码6

7.实现代码

/**
 * @brief 卡尔曼滤波
 *
 * @param Zk 测量值
 * @return float 输出值
 */
float Kalman_filtering(float Zk)
{

    static float Q = 0.001; // 代码01 过程噪声协方差矩阵
    static float R = 0.05;  // 代码02 测量噪声的协方差

    static float Pk = 1; //
    static float Kg = 1; //
    static float Xk = 0; //

    // 1.先验估计计算
    // Xk = X(k-1); // 代码1

    // 2.预测协方差矩阵
    Pk = Pk + Q; // 代码2

    // 3.建立测量方程
    // Zk = Zk; // 代码3

    // 4.计算卡尔曼增益
    Kg = Pk / (Pk + R); // 代码4

    // 5.计算当前最优估计值
    Xk = Xk + Kg * (Zk - Xk); // 代码5

    // 6.更新协方差矩阵
    Pk = (1 - Kg) * Pk; // 代码6

    return Xk;
}

卡尔曼滤波需要我们来调教,分别调节下面的参数,会获取到不同的滤波效果

static float Q = 0.001; // 代码01 过程噪声协方差矩阵
static float R = 0.05;  // 代码02 测量噪声的协方差

注意:例如一个温度采集,这个温度是随机变化的,和时间变化没有关系,所以上面的计算中并没有使用到dt变量(采样时间间隔=100ms,500ms,1000ms…),但是这个时间间隔对参数迭代会有影响,因此dt同样会影响到滤波效果