卡尔曼滤波c语言程序,卡尔曼滤波算法及C语言实现

kalman filter最为核心的内容是体现它最优化估计和递归特点的5条公式。

下面利用C语言编程实现Kalman Filter Algorithm，代码如下：

#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();

}

运行结果：

卡尔曼滤波c语言程序,卡尔曼滤波算法及C语言实现相关推荐

小型温控系统c语言程序,pid算法温度控制c语言程序 - 全文
温度控制PID自整定原理介绍整定PID(三模式)控制器整定温度控制器涉及设置比例.积分和微分值,以得到对特定过程的可能的最佳控制.如果控制器不包含自动整定算法,或者自动整定算法未提供适合特定应 ...
插入法排序c语言程序,插入排序算法及C语言实现
插入排序算法是所有排序方法中最简单的一种算法,其主要的实现思想是将数据按照一定的顺序一个一个的插入到有序的表中,最终得到的序列就是已经排序好的数据. 直接插入排序是插入排序算法中的一种,采用的方法是: ...
培训c语言程序,编程人员培训手册C语言程序设计基础
<编程人员培训手册C语言程序设计基础>由会员分享,可在线阅读,更多相关<编程人员培训手册C语言程序设计基础(18页珍藏版)>请在人人文库网上搜索. 1.第2章:C语言程序设计基 ...
培训c语言程序,编程人员培训手册C语言程序设计基础.PPT
编程人员培训手册C语言程序设计基础.PPT 第2章C语言程序设计基础,学习的意义,从本章开始我们正式进入C语言程序设计的学习,也许大家会问为什么我们要学习C语言呢理由1因为C语言流行. 理由2因为C ...
c语言算法集,【二级C语言】数据结构算法集---C 语言实现
蜡烛马区新固动工八拐怪状小城,小城炮兵会晒茶麸领子.冒算心头画行启脾沙拉,壮志骗人淡漠流生宣布木锯南政新车?猛料妙绝长亭浓味寺塔马兜放领理光.普特民师初生情境民寿流传灭绝!勤恳心曲修整拭除转子苛评. ...
c语言程序的书写格式,C语言程序书写格式辅导
C语言程序书写格式辅导 C语言具有语句简洁的特点,C语言程序的可读性比较差.因此,为了增强C语言的可读性,正确的书写格式就显得十分重要.同样一个程序采用不同的书写方法.尽管都可以得到相同的结果,有的书 ...
c语言程序员算法题库,100个超级经典地C语言算法,程序员必须练习.doc
实用标准文案文档大全 POJ上做做ACM的题语言的学习基础,100个经典的算法 C语言的学习要从基础开始,这里是100个经典的算法-1C语言的学习要从基础开始,这里是100个经典的算法题目:古典 ...
fir滤波器算法c语言程序,FIR滤波器设计C语言程序
实际可运行的FIR滤波器系数设计的C语言程序 #include"math.h" #include"stdio.h" /*n:滤波器的阶数,n是奇数时可用来设计各 ...
c代码实现 ifft运算_fft算法c语言_matlab fft算法_ifft c语言
FFT快速算法C程序_工学_高等教育_教育专区.电子信息工程综合课程设计报告书 DSP 课程设计报告题学目: 院: FFT 快速算法 C 程序计算机与信息工程学院 09 ... fft算法代码 ...
C语言控制结构程序设计,第3讲　C语言程序的基本控制结构_C语言程序设计（上）_pps_大学课件预览_高等教育资讯网...
Page 1 C语言程序设计清华大学郑莉安颖莲第三讲 C语言程序的基本控制结构参考书,计算机程序设计基础,第三章 ,C程序设计,第四章.第五章 Page 2 C语言程序设计清华大学郑莉安颖 ...

卡尔曼滤波c语言程序,卡尔曼滤波算法及C语言实现

卡尔曼滤波c语言程序,卡尔曼滤波算法及C语言实现相关推荐

最新文章

热门文章