卡尔曼滤波器(Kalman filter)是一种高效的递归滤波器, 能够从一系列包含噪音的测量值中估计动态系统的状态. 因为不需要存储历史状态, 没有复杂计算, 非常适合在资源有限的嵌入式系统中使用. 常用于飞行器的导引, 导航及控制, 机械和金融中的时间序列分析, 轨迹最佳化等. 卡尔曼滤波不需要假设误差是正态分布, 但如果误差属于正态分布, 卡尔曼滤波的结果会更为准确.
卡尔曼滤波的计算分二个步骤: 预测与更新. 在预测阶段, 滤波器基于上一步的预测结果, 预测当前状态和误差; 在更新阶段, 滤波器利用当前的测量值和预测值, 计算得到新的状态值和误差.
- Original Error Estimate, calculate the Kalman Gain using Error in Estimate and Error in Data(Measurement)
预测阶段, 用预测误差和测量误差计算卡尔曼增益 - Original Estimate, calculate Current Estimate using Kalman Gain, Previous Estimate and Measured Value
更新阶段, 结合测量值, 用卡尔曼增益计算当前的状态 - Calculate the new Error in Estimate
计算新的预测误差
定义
完整的卡尔曼滤波定义是这样的
-
Predict step 预测阶段
- State prediction 预测系统状态:
x t ∣ t − 1 ^ = F t x t − 1 ∣ t − 1 ^ + B t u t \hat{x_{t|t-1}} = F_t \hat{x_{t-1|t-1}} + B_t u_t xt∣t−1^=Ftxt−1∣t−1^+Btut - Uncertainty prediction 预测误差:
P t ∣ t − 1 = F t P t − 1 ∣ t − 1 F t T + Q t P_{t|t-1} = F_t P_{t-1|t-1} F_t^T + Q_t Pt∣t−1=FtPt−1∣t−1FtT+Qt
- State prediction 预测系统状态:
-
Update step 更新阶段
- Kalman gain 更新卡尔曼增益:
K t = P t ∣ t − 1 H t T H t P t ∣ t − 1 H t T + R t K_t = \frac{P_{t|t-1} H_t^T} {H_t P_{t|t-1} H_t^T + R_t} Kt=HtPt∣t−1HtT+RtPt∣t−1HtT - State update 更新状态:
x t ∣ t ^ = x t ∣ t − 1 ^ + K t ( z t − H t x t ∣ t − 1 ^ ) \hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - H_t \hat{x_{t|t-1}}) xt∣t^=xt∣t−1^+Kt(zt−Htxt∣t−1^) - Uncertainty update 更新误差:
P t ∣ t = ( I − K t H t ) P t ∣ t − 1 P_{t|t} = (I - K_t H_t) P_{t|t-1} Pt∣t=(I−KtHt)Pt∣t−1
- Kalman gain 更新卡尔曼增益:
对以上符号的说明
- x ^ \hat{x} x^: 预测的系统状态向量
The state vector, which represents the true state of the system that we want to estimate. - t t t: 时间序列
The time step index, corresponding to different time periods. - F t F_t Ft: 状态转移矩阵
The state transition matrix, which models how the system evolves from time step t − 1 t-1 t−1 to t t t without taking into account external factors. - B t B_t Bt: 控制输入矩阵
The control input matrix, used to incorporate the effect of any external factors u t u_t ut (e.g., motors or steer engines inputs). - u t u_t ut: 控制输入向量
The control input vector, containing the external factors impacting the system. - P P P: 误差矩阵
The uncertainty (covariance) matrix, which quantifies our uncertainty about the estimated state. - Q t Q_t Qt: 过程噪声协方差矩阵
The process noise covariance matrix, representing the estimation error caused by our simplified model of the system state dynamics. Q矩阵表示系统模型的过程噪声, 系统模型是一个近似值, 在系统状态的整个生命周期中, 系统模型的准确性会发生波动, Q矩阵用于表示这种不确定性, 并增加了状态上的现有噪声. 例如飞行器电机的震动给加速度的读数带来的误差. - H t H_t Ht: 观察值转换矩阵
The observation matrix, which models how the true system state is transformed into the observed system state. - K t K_t Kt: 卡尔曼增益
The Kalman gain, which determines how much we trust the new observation relative to our prediction. 卡尔曼增益是一个介于0到1之间的数, 用于表示在预测中观察值所占的比重, 卡尔曼增益越大说明噪声越大, 观察值越重要. - z t z_t zt: 观察值(测量值)向量
The observation (measurement) vector, containing the recorded states. - R t R_t Rt: 测量噪声协方差矩阵
测量噪声指的是测量工具(如传感器)测量时的固有噪声, 例如在静止时加速度传感器读数的上下波动, The observation noise covariance matrix, representing the measurement noise in the observed states. - I I I: 单位矩阵
The identity matrix.
简化
对于一个静止(或匀速运动)的物体观测加速度和角速度, 可以忽略控制输入 B t B_t Bt 和 u t u_t ut, 将 F t F_t Ft, H t H_t Ht视为单位矩阵, 卡尔曼计算公式可以简化为
-
Predict step 预测阶段,
- State prediction 预测状态等于前一步的状态:
x t ∣ t − 1 ^ = x t − 1 ∣ t − 1 ^ \hat{x_{t|t-1}} = \hat{x_{t-1|t-1}} xt∣t−1^=xt−1∣t−1^ - Uncertainty prediction 预测误差等于更新后的误差加上过程噪声:
P t ∣ t − 1 = P t − 1 ∣ t − 1 + Q t P_{t|t-1} = P_{t-1|t-1} + Q_t Pt∣t−1=Pt−1∣t−1+Qt
- State prediction 预测状态等于前一步的状态:
-
Update step 更新阶段,
- Kalman gain 更新卡尔曼增益:
K t = P t ∣ t − 1 P t ∣ t − 1 + R t K_t = \frac{P_{t|t-1}} {P_{t|t-1} + R_t} Kt=Pt∣t−1+RtPt∣t−1 - State update 更新状态:
x t ∣ t ^ = x t ∣ t − 1 ^ + K t ( z t − x t ∣ t − 1 ^ ) \hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - \hat{x_{t|t-1}}) xt∣t^=xt∣t−1^+Kt(zt−xt∣t−1^) - Uncertainty update 更新误差:
P t ∣ t = ( I − K t ) P t ∣ t − 1 P_{t|t} = (I - K_t) P_{t|t-1} Pt∣t=(I−Kt)Pt∣t−1
- Kalman gain 更新卡尔曼增益:
实例
1. 初始化
令预测误差初始值为 P = 10000 P = 10000 P=10000
测量误差 σ = 0.1 σ = 0.1 σ=0.1，方差 σ 2 = 0.01 σ^2 = 0.01 σ2=0.01, 即 R R R 为固定的 0.01
噪声方差为 q = 0.15 q = 0.15 q=0.15
令初始预测值 $ \hat{x} = 10 $
预测误差 $ P = P + q = 10000 + 0.15 = 10000.15 $
2. 观察值 Z = 50.486 Z = 50.486 Z=50.486
卡尔曼增益
K = P P + r = 10000.15 10000.15 + 0.01 = 0.99999 K = \frac{P}{P + r} = \frac{10000.15}{10000.15 + 0.01} = 0.99999 K=P+rP=10000.15+0.0110000.15=0.99999
更新系统状态(等于预测状态)
x ^ = x ^ + K ∗ ( Z − x ^ ) = 10 + 0.99999 ∗ ( 50.486 − 10 ) = 50.486 \hat{x} = \hat{x} + K * (Z - \hat{x}) = 10 + 0.99999 * (50.486 - 10) = 50.486 x^=x^+K∗(Z−x^)=10+0.99999∗(50.486−10)=50.486
更新预测误差
P = ( 1 − K ) ∗ P = ( 1 − 0.99999 ) ∗ 10000.15 = 0.01 P = (1 - K) * P = (1 - 0.99999) * 10000.15 = 0.01 P=(1−K)∗P=(1−0.99999)∗10000.15=0.01
P = P + q = 0.01 + 0.15 = 0.16 P = P + q = 0.01 + 0.15 = 0.16 P=P+q=0.01+0.15=0.16
3. 观察值 Z = 50.963 Z = 50.963 Z=50.963
卡尔曼增益
K = P P + r = 0.16 0.16 + 0.01 = 0.9412 K = \frac{P}{P + r} = \frac{0.16}{0.16 + 0.01} = 0.9412 K=P+rP=0.16+0.010.16=0.9412
更新系统状态(等于预测状态)
x ^ = x ^ + K ∗ ( Z − x ^ ) = 50.486 + 0.9412 ∗ ( 50.963 − 50.486 ) = 50.934 \hat{x} = \hat{x} + K * (Z - \hat{x}) = 50.486 + 0.9412 * (50.963 - 50.486) = 50.934 x^=x^+K∗(Z−x^)=50.486+0.9412∗(50.963−50.486)=50.934
更新预测误差
P = ( 1 − K ) ∗ P = ( 1 − 0.9412 ) ∗ 0.16 = 0.0094 P = (1 - K) * P = (1 - 0.9412) * 0.16 = 0.0094 P=(1−K)∗P=(1−0.9412)∗0.16=0.0094
P = P + q = 0.0094 + 0.15 = 0.1594 P = P + q = 0.0094 + 0.15 = 0.1594 P=P+q=0.0094+0.15=0.1594
可以看到 P P P 和 K K K 的值迅速收敛
实现
一个简单的C语言演示代码, 会输出每次迭代后产生的K增益, P误差和预测值.
#include <stdio.h>const int measures[] = {-269, -255, -130, 228, -437, -1234, 1247, 173, -400, -1561, -1038, 207, 958, -516, -581, -716, -18, -1193, -989, -593, 484, 102, 718, 1362, 1563, 2683, 428, 1616, 2922, 2968, 3046, 3572, 4006, 4821, 3964, 3127, 3086, 3190, 3682, 4015, 4471, 4211, 4523, 5098, 6452, 5947, 6150, 5694, 6498, 7048, 7519, 6820, 5652, 6608, 7409, 8729, 10569, 10760, 9054, 9856, 8656, 7972, 9320, 6958, 6820, 7391, 7702, 8248, 9426, 8812, 8666, 8838, 7943, 6878, 7233, 7536, 8381, 8314, 7267, 6704, 7343, 6321, 6409, 6023, 7334, 7975, 7659, 6159, 5990, 6187, 6645, 6702, 6273, 7196, 7381, 6939, 4201, 4108, 5338, 6469, 4528, 3679, 4113, 4158, 3428, 2966, 3466, 3704, 3220, 2582, 2818, 3039, 2835, 1929, 1362, 890, 396, -201, -992, -1502, -2009, -1667, -1503, -1881, -2713, -3231, -2856, -2868, -2989, -4140, -4878, -4690, -3838, -4244, -5312, -9966, -6514, -5246, -4559, -4832, -6833, -8869, -9207, -8021, -7959, -9219, -10911, -12606, -12296, -11710, -10460, -10827, -13095, -12183, -10989, -9458, -9520, -10622, -12221, -11792, -9510, -7964, -7935, -8728, -9137, -8076, -6628, -6379, -7132, -8076, -7499, -6536, -5855, -6285, -7310, -7517, -7217, -6997, -6440, -5806, -4647, -4006, -4144, -3800, -2820, -1811, 215, 768, 531, 186, 514, 2117, 2618, 2396, 1600, 1477, 1800, 2329, 2015, 1585, 1461
};static float k_gain, r_noise, q_noise, x_est, p_err, z_measure;void Kalman_Init(void)
{p_err = 1.0;r_noise = 10.0;q_noise = 1;x_est = -200.0;p_err = p_err + q_noise;
}float Kalman_Update(float measure)
{k_gain = p_err / (p_err + r_noise);x_est = x_est + k_gain * (measure - x_est);p_err = (1 - k_gain) * p_err;p_err = p_err + q_noise;return x_est;
}int main(int argc, char *const argv[])
{int i;float estimate, new_measure;Kalman_Init();for (i = 0; i < sizeof(measures)/sizeof(int); i++){estimate = Kalman_Update((float)measures[i]);printf("%3d: %6d %10.5f %10.5f %10.5f\r\n", i, measures[i], k_gain, p_err, estimate);}return 0;
}
对参数的说明
measures数值来自于手持物体旋转时陀螺仪传感器的真实读数, 本例中陀螺仪的实测噪声 R R R在10至20这个数量级, 运动中的抖动来源于手持产生的抖动p_err = 1.0;和x_est = -200.0;, 预测和误差的初始值可以随意取一个接近的值, 如果不知道取什么值, 设为0也问题不大.r_noise = 10.0;和q_noise = 1;这两个值会显著影响结果, 其中r_noise可以使用传感器收集静止状态数据后计算方差得到,q_noise无法明确计算, 初始可以赋0.1至1之间的数.
输出结果格式
0: -269 0.04762 0.97619 -12.809521: -255 0.08894 1.38937 -34.349242: -130 0.12199 1.71988 -46.017523: 228 0.14675 1.96749 -5.805664: -437 0.16440 2.14403 -76.695335: -1234 0.17655 2.26550 -281.017646: 1247 0.18471 2.34705 1.215127: 173 0.19009 2.40090 33.869718: -400 0.19361 2.43607 -50.130489: -1561 0.19589 2.45887 -346.0908210: -1038 0.19736 2.47359 -482.6455111: 207 0.19831 2.48306 -345.8844312: 958 0.19891 2.48915 -86.5228013: -516 0.19930 2.49305 -172.1196414: -581 0.19955 2.49555 -253.7136815: -716 0.19971 2.49715 -346.0391816: -18 0.19982 2.49818 -280.4912117: -1193 0.19988 2.49883 -462.88641
...
使用不同的 r_noise 和 q_noise 得到的变化曲线如图
图中变化最剧烈的蓝色曲线是从传感器得到的原始测量值, 可以看到原始数据的抖动是很明显的, 经过卡尔曼滤波后可以明显消除抖变, 使结果数据更平滑.
通过变换多种噪声组合, 可以观察到的现象有
- 在
r_noise和q_noise不变的情况下, 不管p_err和x_est设置什么初始值, 都会很快收敛, 最后输出相同的结果序列(这点没有在本例体现, 需要自己验证) r_noise越大表示测量噪声越大, 测量值的权重越低,r_noise越大, 结果越平滑但是延迟也越大q_noise是系统的固有误差,q_noise越小, 结果越平滑延迟越大r_noise和q_noise等比例变化时, 产生的结果序列不变, 图中 r=10,q=0.5 和 r=20,q=1 这两个曲线是重合的.
总结
从卡尔曼滤波器的定义看
- 整个过程中, 对状态 x ^ \hat{x} x^ 的预测和更新, 除了自身和观测值 z t z_t zt之外, 关系到这几个参数 F t , B t , u t , K t , H t F_t, B_t, u_t, K_t, H_t Ft,Bt,ut,Kt,Ht, 其中 F t , u t , H t F_t, u_t, H_t Ft,ut,Ht 在系统中都相对固定, 而 B t B_t Bt 是已知输入, 例如电机或舵机的动作, 已知且确定的, 不存在噪声, 真正起作用的是 K t K_t Kt 这个参数.
- 而 K t K_t Kt 这个参数的计算, 和 x ^ \hat{x} x^ 没关系. 系统中不存在反馈, 观测值 z t z_t zt 和预测值 x ^ \hat{x} x^ 都不会影响 K t K_t Kt, 只要 H t , R t , Q t H_t, R_t, Q_t Ht,Rt,Qt 这三个值固定, 最后 K t K_t Kt 会收敛为一个常量
当符合上面两点条件时, 状态的更新公式就变成下面的式子
x t ∣ t ^ = x t ∣ t − 1 ^ + K t ( z t − x t ∣ t − 1 ^ ) = x t ∣ t − 1 ^ + K t z t − K t x t ∣ t − 1 ^ = ( 1 − K t ) x t ∣ t − 1 ^ + K t z t \hat{x_{t|t}} = \hat{x_{t|t-1}} + K_t (z_t - \hat{x_{t|t-1}}) \\ = \hat{x_{t|t-1}} + K_t z_t - K_t \hat{x_{t|t-1}} \\ = (1 - K_t) \hat{x_{t|t-1}} + K_t z_t xt∣t^=xt∣t−1^+Kt(zt−xt∣t−1^)=xt∣t−1^+Ktzt−Ktxt∣t−1^=(1−Kt)xt∣t−1^+Ktzt
令 β = 1 − K t \beta = 1 - K_t β=1−Kt, 这就是一个典型的离散序列差分方程(difference equation)构成的低通滤波器
x t ∣ t ^ = β x t ∣ t − 1 ^ + ( 1 − β ) z t \hat{x_{t|t}} = \beta \hat{x_{t|t-1}} + (1 - \beta) z_t xt∣t^=βxt∣t−1^+(1−β)zt
在实际使用中, Q Q Q和 R R R大概率是常数, 增益 K t K_t Kt会快速收敛, 上面的式子更简单, 更容易理解和实现, 也符合它的典型使用方式, 即手动调整 β \beta β (等价于调整 Q Q Q和 R R R), 在延迟和平滑之间找到最佳平衡.
参考
- 卡尔曼滤波 非常好的介绍 https://www.kalmanfilter.net/CN/alphabeta_cn.html
- 扩展卡尔曼滤波 https://simondlevy.github.io/ekf-tutorial/
- 概念说明和Python实现 https://forecastegy.com/posts/kalman-filter-for-time-series-forecasting-in-python/
- 另一篇浅显易懂的卡尔曼滤波器说明 https://thekalmanfilter.com/kalman-filter-explained-simply/
