kalman滤波器的实现
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
1、c语言实现
1//1. 结构体类型定义
2typedef struct
3{
4 float LastP;//上次估算协方差 初始化值为0.02
5 float Now_P;//当前估算协方差 初始化值为0
6 float out;//卡尔曼滤波器输出 初始化值为0
7 float Kg;//卡尔曼增益 初始化值为0
8 float Q;//过程噪声协方差 初始化值为0.001
9 float R;//观测噪声协方差 初始化值为0.543
10}KFP；//Kalman Filter parameter
11
12//2. 以高度为例 定义卡尔曼结构体并初始化参数
13KFP KFP_height={0.02,0,0,0,0.001,0.543};
14
15/**
16 *卡尔曼滤波器
17 *@param KFP *kfp 卡尔曼结构体参数
18 * float input 需要滤波的参数的测量值（即传感器的采集值）
19 *@return 滤波后的参数（最优值）
20 */
21 float kalmanFilter(KFP *kfp,float input)
22 {
23 //预测协方差方程：k时刻系统估算协方差 = k‐1时刻的系统协方差 + 过程噪声协方差
24 kfp‐>Now_P = kfp‐>LastP + kfp‐>Q;
25 //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 +
观测噪声协方差）
26 kfp‐>Kg = kfp‐>Now_P / (kfp‐>NOw_P + kfp‐>R);
27 //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 *
（测量值 ‐ 状态变量的预测值）
28 kfp‐>out = kfp‐>out + kfp‐>Kg * (input ‐kfp‐>out);//因为这一次的预测值就
是上一次的输出值
29 //更新协方差方程: 本次的系统协方差付给 kfp‐>LastP 威下一次运算准备。
30 kfp‐>LastP = (1‐kfp‐>Kg) * kfp‐>Now_P;
31 return kfp‐>out；
32 }
2、初值
卡尔曼的初值比较重要，把最新的一次测量值作为初始值赋值进去，下一次采样的时候，两次
差值比较小，逼近过程比较快。
如果初值设为0，滤波曲线和实际曲线的逼近过程可能会比较长。