卡尔曼滤波算法及C语言实现
来源:互联网 发布:郑州大学淘宝地址 编辑:程序博客网 时间:2024/06/13 16:19
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();}
运行结果:
0 1
- 卡尔曼滤波算法及C语言实现
- 卡尔曼滤波 C语言
- 卡尔曼滤波简单理解及C语言代码
- C语言实现卡尔曼滤波(转)
- 卡尔曼滤波简介及算法实现代码
- 卡尔曼滤波原理及实现
- 卡尔曼滤波算法
- 卡尔曼滤波算法
- 卡尔曼滤波算法
- 卡尔曼滤波简介+ 算法实现代码(C/C++)(转)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- [教程] 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波简介及其算法实现代码(C++/C/MATLAB)
- 卡尔曼滤波算法原理
- Android客户端与服务器交互中的token
- [python]用eval强制将字符串转换为字典变量时候出错:NameError: name 'null' is not defined[python]用eval函数 字符串转dict
- Android无限极树状(tree)结构Demo
- 欢迎使用CSDN-markdown编辑器
- Pocket Hacking: NetHunter实战指南
- 卡尔曼滤波算法及C语言实现
- android studio 安装指南
- 0-1背包问题和部分背包(fractional knapsack)问题分析
- nptq使用
- 带你玩转Visual Studio——带你理解多字节编码与Unicode码
- Python中的类方法和静态方法
- FFmpeg音频滤镜
- css中的边框border
- 荧火祭与流年—三月总结