卡尔曼滤波器的定义,实例和代码实现
卡尔曼滤波器(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 预测系统状态:
- Uncertainty prediction 预测误差:
- State prediction 预测系统状态:
-
Update step 更新阶段
- Kalman gain 更新卡尔曼增益:
- State update 更新状态:
- Uncertainty update 更新误差:
- Kalman gain 更新卡尔曼增益:
对以上符号的说明
- : 预测的系统状态向量
The state vector, which represents the true state of the system that we want to estimate. - : 时间序列
The time step index, corresponding to different time periods. - : 状态转移矩阵
The state transition matrix, which models how the system evolves from time step to without taking into account external factors. - : 控制输入矩阵
The control input matrix, used to incorporate the effect of any external factors (e.g., motors or steer engines inputs). - : 控制输入向量
The control input vector, containing the external factors impacting the system. - : 误差矩阵
The uncertainty (covariance) matrix, which quantifies our uncertainty about the estimated state. - : 过程噪声协方差矩阵
The process noise covariance matrix, representing the estimation error caused by our simplified model of the system state dynamics. Q矩阵表示系统模型的过程噪声, 系统模型是一个近似值, 在系统状态的整个生命周期中, 系统模型的准确性会发生波动, Q矩阵用于表示这种不确定性, 并增加了状态上的现有噪声. 例如飞行器电机的震动给加速度的读数带来的误差. - : 观察值转换矩阵
The observation matrix, which models how the true system state is transformed into the observed system state. - : 卡尔曼增益
The Kalman gain, which determines how much we trust the new observation relative to our prediction. 卡尔曼增益是一个介于0到1之间的数, 用于表示在预测中观察值所占的比重, 卡尔曼增益越大说明噪声越低, 观察值越重要. - : 观察值(测量值)向量
The observation (measurement) vector, containing the recorded states. - : 测量噪声协方差矩阵
测量噪声指的是测量工具(如传感器)测量时的固有噪声, 例如在静止时加速度传感器读数的上下波动, The observation noise covariance matrix, representing the measurement noise in the observed states. - : 单位矩阵
The identity matrix.
简化
对于一个静止(或匀速运动)的物体观测加速度和角速度, 可以忽略控制输入 和 , 将 , 视为单位矩阵, 卡尔曼计算公式可以简化为
-
Predict step 预测阶段,
- State prediction 预测状态等于前一步的状态:
- Uncertainty prediction 预测误差等于更新后的误差加上过程噪声:
- State prediction 预测状态等于前一步的状态:
-
Update step 更新阶段,
- Kalman gain 更新卡尔曼增益:
- State update 更新状态:
- Uncertainty update 更新误差:
- Kalman gain 更新卡尔曼增益:
实例
1. 初始化
令预测误差初始值为
测量误差,方差 , 即 为固定的 0.01
噪声方差为
令初始预测值
预测误差
2. 观察值
卡尔曼增益
更新系统状态(等于预测状态)
更新预测误差
3. 观察值
卡尔曼增益
更新系统状态(等于预测状态)
更新预测误差
可以看到 和 的值迅速收敛
实现
一个简单的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
数值来自于手持物体旋转时陀螺仪传感器的真实读数, 本例中陀螺仪的实测噪声在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.80952 1: -255 0.08894 1.38937 -34.34924 2: -130 0.12199 1.71988 -46.01752 3: 228 0.14675 1.96749 -5.80566 4: -437 0.16440 2.14403 -76.69533 5: -1234 0.17655 2.26550 -281.01764 6: 1247 0.18471 2.34705 1.21512 7: 173 0.19009 2.40090 33.86971 8: -400 0.19361 2.43607 -50.13048 9: -1561 0.19589 2.45887 -346.09082 10: -1038 0.19736 2.47359 -482.64551 11: 207 0.19831 2.48306 -345.88443 12: 958 0.19891 2.48915 -86.52280 13: -516 0.19930 2.49305 -172.11964 14: -581 0.19955 2.49555 -253.71368 15: -716 0.19971 2.49715 -346.03918 16: -18 0.19982 2.49818 -280.49121 17: -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 这两个曲线是重合的.
总结
从卡尔曼滤波器的定义看
- 整个过程中, 对状态 的预测和更新, 除了自身和观测值之外, 关系到这几个参数 , 其中 在系统中都相对固定, 而 是已知输入, 例如电机或舵机的动作, 已知且确定的, 不存在噪声, 真正起作用的是 这个参数.
- 而 这个参数的计算, 和 没关系. 系统中不存在反馈, 观测值 和预测值 都不会影响 , 只要 这三个值固定, 最后 会收敛为一个常量
当符合上面两点条件时, 状态的更新公式就变成下面的式子
令 , 这就是一个典型的离散序列差分方程(difference equation)构成的低通滤波器
在实际使用中, 和大概率是常数, 增益会快速收敛, 上面的式子更简单, 更容易理解和实现, 也符合它的典型使用方式, 即手动调整 (等价于调整和), 在延迟和平滑之间找到最佳平衡.
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 一个费力不讨好的项目,让我损失了近一半的绩效!
· 清华大学推出第四讲使用 DeepSeek + DeepResearch 让科研像聊天一样简单!
· 实操Deepseek接入个人知识库
· 易语言 —— 开山篇
· 【全网最全教程】使用最强DeepSeekR1+联网的火山引擎,没有生成长度限制,DeepSeek本体
2021-02-28 USB至串口TTL转接设备及Console线