|
卡尔曼滤波一、滤波效果展示 ①. X k-1|k-1 为k-1时刻的输出。 ②. 当X为一维数据时，Fk的值是1。 ③. 一维数据下（uk=0时）：系统预测值 = 系统状态变量k-1时刻的最优值。 2. 预测协方差方程（1）目的 根据 k-1时刻的系统协方差 预测 k时刻系统协方差。
（2）方程
（3）备注 ①. 当X为一维数据时，Fk的值是1。
3. 卡尔曼增益方程（1）目的 根据（k时刻） 协方差矩阵的预测值 计算 卡尔曼增益。
（2）方程
（3）备注 ①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
4. 跟新最优值方程（卡尔曼滤波的输出）（1）目的 根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。
（2）方程
（3）备注 ①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
5. 更新协方差方程（1）目的 为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）
（2）方程
（3）备注 ①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。
四、C 程序代码实现1. 参数列表
2. 代码实现（一维数据滤波） 实际参数是参照别人已经选好的参数，不过也可以自己改变参数，去观察波形的效果，体会每个参数对于滤波效果的影响，这里不详细介绍。
- //1. 结构体类型定义
- typedef struct
- {
-
- float LastP;//上次估算协方差 初始化值为0.02
- float Now_P;//当前估算协方差 初始化值为0
- float out;//卡尔曼滤波器输出 初始化值为0
- float Kg;//卡尔曼增益 初始化值为0
- float Q;//过程噪声协方差 初始化值为0.001
- float R;//观测噪声协方差 初始化值为0.543
- }KFP；//Kalman Filter parameter
- //2. 以高度为例 定义卡尔曼结构体并初始化参数
- KFP KFP_height={
- 0.02,0,0,0,0.001,0.543};
- /** *卡尔曼滤波器 *@param KFP *kfp 卡尔曼结构体参数 * float input 需要滤波的参数的测量值（即传感器的采集值） *[url=/u/return]@return[/url] 滤波后的参数（最优值） */
- float kalmanFilter(KFP *kfp,float input)
- {
-
- //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
- kfp->Now_P = kfp->LastP + kfp->Q;
- //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
- kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
- //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
- kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
- //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
- kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
- return kfp->out；
- }
- /** *调用卡尔曼滤波器 实践 */
- int height;
- int kalman_height=0;
- kalman_height = kalmanFilter(&KFP_height,(float)height);
| 
