2.一维卡尔曼滤波(动态模型)
目录
- 1. 使用场景
- 2. 流程图
- 1. 先验估计计算
- 2. 预测协方差矩阵
- 3.建立测量方程
- 4.计算卡尔曼增益
- 5.计算当前最优估计值
- 6.更新协方差矩阵
- 7.实现代码
1.一维卡尔曼滤波(恒定的动态模型)
2.一维卡尔曼滤波(动态模型)
3.二维-卡尔曼滤波算法
1. 使用场景
测量的是1维的数据,只有一个测量值(例如:电压,电流,温湿度采集)----采集的数据是随机的
和测量硬币直径不同-----硬币的直径是固定的
2. 流程图
下面是一维的卡尔曼滤波示例,由于一维的特殊性,有些变量是1或者没有,导致公式省略了好多,
建议你先去研究一下3.二维-卡尔曼滤波算法,弄明白了再回来看一维的,就简单明了了
1. 先验估计计算
📌 固定公式 :
X k = A ⋅ X k − 1 + B ⋅ U k + W k \displaystyle{{X}_{k}}=A⋅{{X}_{k-1}}+B⋅{{U}_{k}}+{{W}_{k}} Xk=A⋅Xk−1+B⋅Uk+Wk
• X k : 是状态向量 \displaystyle {{X}_{k}} : \text{是状态向量} Xk:是状态向量
• A = [ 1 ] : 是状态转移矩阵 \displaystyle {A}=\left [{\begin{matrix}1\end{matrix}}\right ]: \text{是状态转移矩阵} A=[1]:是状态转移矩阵
• B = [ 0 ] : 是输入矩阵 ( 没有 ) \displaystyle {B}=\left [{\begin{matrix}0\end{matrix}}\right ]:是输入矩阵(没有) B=[0]:是输入矩阵(没有)
• U k : 是控制输入向量 ( 没有 ) \displaystyle {{U}_{k}}:是控制输入向量(没有) Uk:是控制输入向量(没有)
• W k : 是过程噪声向量 ( 省略 ) \displaystyle {{W}_{k}}:是过程噪声向量(省略) Wk:是过程噪声向量(省略)
📌 状态方程就可以写作：
X k = A ⋅ X k − 1 \displaystyle{{X}_{k}}=A⋅{{X}_{k-1}} Xk=A⋅Xk−1
X k = X k − 1 \displaystyle{{X}_{k}}={{X}_{k-1}} Xk=Xk−1 //代码1
2. 预测协方差矩阵
📌 固定公式 :
P k = A ⋅ P k − 1 ⋅ A T + Q = P k − 1 + Q \displaystyle{{P}_{k}}=A⋅{{P}_{k-1}}⋅{{A}^{T}}+Q={{P}_{k-1}}+Q Pk=A⋅Pk−1⋅AT+Q=Pk−1+Q
• Q : 过程噪声协方差矩阵 , 需要自定义好 \displaystyle {Q}:过程噪声协方差矩阵,需要自定义好 Q:过程噪声协方差矩阵,需要自定义好
static float Q = 0.01; // 代码01 过程噪声协方差矩阵
📌
P k = P k − 1 + Q \displaystyle{{P}_{k}}={{P}_{k-1}}+Q Pk=Pk−1+Q //代码2
3.建立测量方程
📌 固定公式 :
Z k = H X k + V k \displaystyle{{Z}_{k}}=H{{X}_{k}}+{{V}_{k}} Zk=HXk+Vk
📌
Z k = T e m p ( 传感器获取到的温度 ) / / 代码 3 \displaystyle{{Z}_{k}}=Temp (传感器获取到的温度) //代码3 Zk=Temp(传感器获取到的温度)//代码3
4.计算卡尔曼增益
📌 固定公式 :
K g ( k ) = P k − 1 ⋅ H T H P k − 1 ⋅ H T + R = P k − 1 P k − 1 + R \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} Kg(k)=HPk−1⋅HT+RPk−1⋅HT=Pk−1+RPk−1
• P k − 1 : 是先验状态协方差 ( P k 的初始值 = 1 ) \displaystyle {{P}_{k-1}}:是先验状态协方差({{P}_{k}}的初始值=1) Pk−1:是先验状态协方差(Pk的初始值=1)
• H : 是测量矩阵 [ 1 ] \displaystyle {H}:是测量矩阵\left [{\begin{matrix}1\end{matrix}}\right ] H:是测量矩阵[1]
• R : 是测量噪声的协方差 , 需要自定义好 \displaystyle {R}:是测量噪声的协方差,需要自定义好 R:是测量噪声的协方差,需要自定义好
static float R = 0.05; // 代码02 测量噪声的协方差
📌
K g ( k ) = P k − 1 P k − 1 + R \displaystyle{{K}_{g(k)}}=\frac{{{P}_{k-1}}}{{{P}_{k-1}}+R} Kg(k)=Pk−1+RPk−1 //代码4
R=0时,卡尔曼滤波的曲线就会 和 测量值 Z k \displaystyle {{Z}_{k}} Zk 重合
5.计算当前最优估计值
📌 固定公式 :
X k = X k − 1 + K g ( k ) ⋅ ( Z k − H ⋅ X k − 1 ) \displaystyle{{X}_{k}}={{X}_{k-1}}+{{K}_{g(k)}}⋅({{Z}_{k}}-H⋅{{X}_{k-1}}) Xk=Xk−1+Kg(k)⋅(Zk−H⋅Xk−1)
H = 1,所以
📌
X k = X k − 1 + K g ( k ) ⋅ ( Z k − X k − 1 ) \displaystyle{{X}_{k}}={{X}_{k-1}}+{{K}_{g(k)}}⋅({{Z}_{k}}-{{X}_{k-1}}) Xk=Xk−1+Kg(k)⋅(Zk−Xk−1) //代码5
6.更新协方差矩阵
📌 固定公式 :
P k = [ I − K g ( k ) ⋅ H ] ⋅ P k − 1 \displaystyle{{P}_{k}}=[I-{{K}_{g(k)}}⋅H]⋅{{P}_{k-1}} Pk=[I−Kg(k)⋅H]⋅Pk−1
I \displaystyle I I:单位矩阵 [ 1 ] \displaystyle\left [{\begin{matrix}1\end{matrix}}\right ] [1]
📌
P k = [ 1 − K g ( k ) ] ⋅ P k − 1 \displaystyle{{P}_{k}}=[1-{{K}_{g(k)}}]⋅{{P}_{k-1}} Pk=[1−Kg(k)]⋅Pk−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; // 代码6return Xk;
}
卡尔曼滤波需要我们来调教,分别调节下面的参数,会获取到不同的滤波效果
static float Q = 0.001; // 代码01 过程噪声协方差矩阵
static float R = 0.05; // 代码02 测量噪声的协方差
注意:例如一个温度采集,这个温度是随机变化的,和时间变化没有关系,所以上面的计算中并没有使用到dt变量(采样时间间隔=100ms,500ms,1000ms…),但是这个时间间隔对参数迭代会有影响,因此dt同样会影响到滤波效果