Kalman 滤波
来源:互联网 发布:mac给视频配音的软件 编辑:程序博客网 时间:2024/06/05 10:23
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
double frand()
{
return 2*((rand ()/(double)RAND_MAX)-0.5);
}
/**卡尔曼滤波算法
* 1、有两个值:一个预测值;另一个为测量值
* 2、预测值P(k) = P(k-1)+R(k) // P(k-1)为上一时刻的预测值,R为预测值的方差;
* 3、测量值T(k) = t(k)+Q(k) // t(k)为k时刻的真实值,Q为测量误差
* 3、R(k)^2 = U^2 + D(k-1)^2 // U为预测值得不可信度; D(k-1)为k-1次最优预估值P(k-1)的偏差
* 4、Kg(k)^2 = R(k)^2 / (R(k)^2 + Q(k)^2)
* 5、D(k)^2 = (1-Kg(k-1))*R(k-1)^2
*
* 最优值X(k) = P(k) + Kg * (P(k)-T(k))
*/
int main(int argc, char *argv[])
{
float x_last =0;
float p_last =0.2;
float R = 0.542;
float Q = 0;//0.018;
float kg;
float x_mid;
float p_mid;
float x_now;
float p_now;
float z_real = 0.56;
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i=0;
x_last = z_real+frand()*0.5;
x_mid = x_last;
FILE *handler = fopen("D:/data.txt","w+");
printf("handler = %d\n",handler);
fprintf (handler,"this is a test program \n");
for(i=0;i<10000;i++)
{
x_mid = x_last; //x_last = x(k-1|k-1) ;x_mid = x(k|k-1)
p_mid = p_last+Q; //p_mid = p(k|k-1) ;p_last = p(k-1|k-1) Q为噪声
kg = p_mid/(p_mid+R); //kg为卡尔曼增益;R为噪声
z_measure = z_real+frand ()*0.3; //测量值
x_now = x_mid+kg*(z_measure-x_mid); //估计出的最优值
p_now = (1-kg)*p_mid; //最优值对应的协方差
fprintf (handler,"%6.3f\t",z_real);
fprintf (handler,"%6.3f\t",z_measure);
fprintf (handler,"%6.3f\n",x_now);
sumerror_kalman += fabs(z_real-x_now); //卡尔曼估计值得累计误差
sumerror_measure += fabs(z_real - z_measure); //真实值与测量值的累计误差
p_last = p_now; //更新协方差
x_last = x_now; //更新状态值
}
fprintf (handler,"总体测量误差 :%f\n",sumerror_measure);
fprintf (handler,"总体卡尔曼滤波误差: %f\n",sumerror_kalman);
fprintf (handler,"卡尔曼误差所占比例:%d%%\n",(int)((sumerror_kalman/sumerror_measure)*100));
fclose(handler);
}
- Kalman滤波
- Kalman滤波
- Kalman滤波
- Kalman滤波
- kalman滤波
- Kalman 滤波
- kalman滤波
- Kalman滤波实验代码
- kalman卡尔曼滤波
- kalman滤波(转载)
- Kalman滤波 Matlab仿真
- (十三)kalman滤波
- Kalman 滤波 跟踪
- Kalman 滤波算法导论
- Kalman滤波原理
- kalman滤波 简介
- kalman滤波--运动跟踪
- Kalman滤波学习资源
- NDK学习之增量更新--编译bsdiff得到差分包
- [Linux][小技巧]提示:WARNING: REMOTE HOST IDENTIFICATION HAS CHANGED!
- windows下安装docker的一些经验之谈
- Android 如何在 ListView 中更新 ProgressBar 进度
- EPOLL内核原理极简图文解读
- Kalman 滤波
- The superclass "javax.servlet.http.HttpServlet" was not found on the JavaBean
- 约瑟夫环问题
- How to Build Your Own Rogue GSM BTS for Fun and Profit
- 简单的文件操作
- mevan配置
- cell自动计算行高遇到回车换行的问题 UITableView+HYBCacheHeight、UITableView+HYBCacheHeight
- LeetCode-492. Construct the Rectangle (Java)
- _sizeof和_countof的区别