大家好,又见面了,我是你们的朋友全栈君。
文章目录
卡尔曼滤波
一、滤波效果展示
蓝色的波形是实际测得的数据,红色的波形是经 Kalman 滤波后的数据波形。
注:这里是实际应用激光测距传感器(TOF)vl53l0x 测得的距离数据。
二、简介
采用递归的方法解决线性滤波问题,只需要当前的测量值和前一个采样周期的估计值就能进行状态估计,需要的存储空间小,每一步的计算量小。
三、组成
1. 预测状态方程
(1)目的:
由 系统状态变量k-1时刻的最优值 和 系统输入 计算出k时刻的 系统预测值。
(2)方程:
(3)备注
①. 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 需要滤波的参数的测量值(即传感器的采集值) *@return 滤波后的参数(最优值) */
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);
五、发送波形到上位机显示
这里使用的是匿名的上位机 V65 版本,具体如何使用可以参考茶大的博客,并且茶大博客里面有上位机的下载地址。茶大博客地址:https://blog.csdn.net/wangjt1988/article/details/83684188
注:文章方程截图及参数来源于中科浩电。
Author : Beyonderwei
Email : Beyonderwei@163.com
Website : http://beyonderwei.com
WeChat:
发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/131023.html原文链接:https://javaforall.cn
【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛
【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...