| 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用 | 您所在的位置：网站首页 › 卡尔曼滤波器原理及应用pdf › 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用 |
卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用
|
大家好，又见面了，我是你们的朋友全栈君。 文章目录卡尔曼滤波一、滤波效果展示二、简介三、组成1. 预测状态方程（1）目的：（2）方程：（3）备注2. 预测协方差方程（1）目的（2）方程（3）备注3. 卡尔曼增益方程（1）目的（2）方程（3）备注4. 跟新最优值方程（卡尔曼滤波的输出）（1）目的（2）方程（3）备注5. 更新协方差方程（1）目的（2）方程（3）备注四、C 程序代码实现1. 参数列表2. 代码实现（一维数据滤波）五、发送波形到上位机显示卡尔曼滤波一、滤波效果展示蓝色的波形是实际测得的数据，红色的波形是经 Kalman 滤波后的数据波形。 注：这里是实际应用激光测距传感器（TOF）vl53l0x 测得的距离数据。  卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用二、简介采用递归的方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能进行状态估计，需要的存储空间小，每一步的计算量小。 三、组成1. 预测状态方程（1）目的：由 系统状态变量k-1时刻的最优值 和 系统输入 计算出k时刻的 系统预测值。 （2）方程： 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用（3）备注①. X k-1|k-1 为k-1时刻的输出。 ②. 当X为一维数据时，Fk的值是1。 ③. 一维数据下（uk=0时）：系统预测值 = 系统状态变量k-1时刻的最优值。 2. 预测协方差方程（1）目的根据 k-1时刻的系统协方差 预测 k时刻系统协方差。 （2）方程 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用（3）备注①. 当X为一维数据时，Fk的值是1。 3. 卡尔曼增益方程（1）目的根据（k时刻） 协方差矩阵的预测值 计算 卡尔曼增益。 （2）方程 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用（3）备注①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。 4. 跟新最优值方程（卡尔曼滤波的输出）（1）目的根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。 （2）方程 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用（3）备注①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。 5. 更新协方差方程（1）目的为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备） （2）方程 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用（3）备注①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。 四、C 程序代码实现1. 参数列表 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用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 : [email protected] Website : http://beyonderwei.com WeChat： 发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/131023.html原文链接：https://javaforall.cn |
【本文地址】