卡尔曼滤波算法--C语言实现

/********************************************************************************* @brief  卡尔曼滤波器 函数* @param  inData - 输入值* @return 滤波后的值* @note   r值固定，q值越大，代表越信任测量值，q值无穷大，代表只用测量值。*                  q值越小，代表越信任模型预测值，q值为0，代表只用模型预测值。*         q:过程噪声，q增大，动态响应变快，收敛稳定性变坏；反之。控制误差 *         r:测量噪声，r增大，动态响应变慢，收敛稳定性变好；反之。控制响应速度*******************************************************************************/
unsigned long KalmanFilter(unsigned long inData)
{static float xdata kalman = 0; //上次卡尔曼值(估计出的最优值)static float xdata p = 10;float xdata q = 0.001; //q:过程噪声float xdata r = 0.001; //r:测量噪声float xdata kg = 0; //kg:卡尔曼增益p += q;kg = p / ( p + r ); //计算卡尔曼增益kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值p = (1 - kg) * p; //更新测量方差return (unsigned long)kalman; //返回估计值
}

实际应用

#include "stdio.h"
#include "stdlib.h"
#include "math.h"double frand()
{return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声
}void main()
{float x_last = 0;float p_last = 0.02;float Q = 0.018;float R = 0.542;float kg;float x_mid;float x_now;float p_mid;float p_now;float z_real = 0.56;float z_measure;float summerror_kalman = 0;float summerror_measure = 0;int i;x_last = z_real + frand()*0.03;x_mid = x_last;for (i = 0; i < 20; i++){x_mid = x_last;p_mid = p_last + Q;kg = p_mid / (p_mid + R);z_measure = z_real + frand()*0.03;//测量值x_now = x_mid + kg * (z_measure - x_mid);//估计出的最有值p_now = (1 - kg)*p_mid;//最优值对应的协方差printf("Real position:%6.3f\n", z_real);printf("Measure position:%6.3f [diff:%.3f]\n", z_measure, fabs(z_real - z_measure));printf("Kalman position: %6.3f [diff:%.3f]\n", x_now, fabs(z_real - x_now));printf("\n\n");summerror_kalman += fabs(z_real - x_now);summerror_measure += fabs(z_real - z_measure);p_last = p_now;x_last = x_now;}printf("总体测量误差 :%f\n", summerror_measure);printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);printf("卡尔曼误差所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));getchar();}

输出结果：

Real position: 0.560
Measure position: 0.564 [diff:0.004]
Kalman position:  0.532 [diff:0.028]Real position: 0.560
Measure position: 0.542 [diff:0.018]
Kalman position:  0.533 [diff:0.027]Real position: 0.560
Measure position: 0.579 [diff:0.019]
Kalman position:  0.538 [diff:0.022]Real position: 0.560
Measure position: 0.565 [diff:0.005]
Kalman position:  0.541 [diff:0.019]Real position: 0.560
Measure position: 0.559 [diff:0.001]
Kalman position:  0.544 [diff:0.016]Real position: 0.560
Measure position: 0.551 [diff:0.009]
Kalman position:  0.545 [diff:0.015]Real position: 0.560
Measure position: 0.584 [diff:0.024]
Kalman position:  0.551 [diff:0.009]Real position: 0.560
Measure position: 0.579 [diff:0.019]
Kalman position:  0.555 [diff:0.005]Real position: 0.560
Measure position: 0.575 [diff:0.015]
Kalman position:  0.558 [diff:0.002]Real position: 0.560
Measure position: 0.540 [diff:0.020]
Kalman position:  0.555 [diff:0.005]Real position: 0.560
Measure position: 0.582 [diff:0.022]
Kalman position:  0.560 [diff:0.000]Real position: 0.560
Measure position: 0.573 [diff:0.013]
Kalman position:  0.562 [diff:0.002]Real position: 0.560
Measure position: 0.561 [diff:0.001]
Kalman position:  0.562 [diff:0.002]Real position: 0.560
Measure position: 0.548 [diff:0.012]
Kalman position:  0.559 [diff:0.001]Real position: 0.560
Measure position: 0.531 [diff:0.029]
Kalman position:  0.555 [diff:0.005]Real position: 0.560
Measure position: 0.535 [diff:0.025]
Kalman position:  0.552 [diff:0.008]Real position: 0.560
Measure position: 0.552 [diff:0.008]
Kalman position:  0.552 [diff:0.008]Real position: 0.560
Measure position: 0.539 [diff:0.021]
Kalman position:  0.549 [diff:0.011]Real position: 0.560
Measure position: 0.540 [diff:0.020]
Kalman position:  0.548 [diff:0.012]Real position: 0.560
Measure position: 0.589 [diff:0.029]
Kalman position:  0.555 [diff:0.005]总体测量误差 :0.312517
总体卡尔曼滤波误差:0.200942
卡尔曼误差所占比例:36%

卡尔曼滤波算法--C语言实现相关推荐

卡尔曼滤波算法c语言stm32,卡尔曼滤波算法及C语言实现_源代码
a往南向北 2019-01-16 20:39:20 11340 收藏 111 分类专栏: C语言嵌入式文章标签: 卡尔曼滤波 C代码卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是 ...
ad 卡尔曼_卡尔曼滤波算法C语言实现(转2)
本帖最后由 jackk 于 2014-3-5 13:00 编辑前段时间在论坛里简单地发了一些关于kalman的理解. 有很多网友顶贴的,趁着今天休息,整理一下前段时间的工作. 有些理解和说法可能不正 ...
卡尔曼滤波算法及c语言代码,卡尔曼滤波算法及C语言代码
卡尔曼滤波算法及C语言代码卡尔曼滤波简介及其算法实现代码卡尔曼滤波算法实现代码(C,C++分别实现) 卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣.所以在这里希望能尽自己能力跟大家讨论一些力 ...
C语言 | 基于51单片机实现MPU6050的卡尔曼滤波算法（代码类2）
github:https://github.com/MichaelBeechan CSDN:https://blog.csdn.net/u011344545 之前写过一个博客(代码分享:单片机开发 | ...
C语言 | 卡尔曼滤波算法2——算法分析
github:https://github.com/MichaelBeechan CSDN:https://blog.csdn.net/u011344545 ===================== ...
卡尔曼滤波算法及C语言实现_源代码
卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有一种就是直接手动编写C语言. 1.前言在google上搜索卡尔曼滤波,很容易找 ...
c语言贝叶斯分类,基于朴素贝叶斯分类器的文本分类算法(C语言)
基于朴素贝叶斯分类器的文本分类算法(C语言) 基于朴素贝叶斯分类器的文本分类算法(C语言).txt两个人吵架,先说对不起的人,并不是认输了,并不是原谅了.他只是比对方更珍惜这份感情.#include ...
解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|3D视觉工坊卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
【转】卡尔曼滤波算法详细推导（相当值得一看）
转载自卡尔曼滤波算法详细推导这一篇对预备知识的介绍还是很好的,过程与原理讲解也很到位,应该是目前看到中文里最好的讲解了. 一.预备知识 1.协方差矩阵是一个维列向量,是的期望 ...
《数据结构与算法 C语言版》—— 3.8习题
本节书摘来自华章出版社<数据结构与算法 C语言版>一书中的第3章,第3.8节,作者:徐凤生,更多章节内容可以访问云栖社区"华章计算机"公众号查看. 3.8习题 1名 ...

卡尔曼滤波算法--C语言实现

卡尔曼滤波算法--C语言实现相关推荐

最新文章

热门文章