搜索
bottom↓
回复: 96

国外网站上找到的简单的卡尔曼滤波算法

  [复制链接]

出0入0汤圆

发表于 2011-8-21 02:02:02 | 显示全部楼层 |阅读模式
折腾了一晚上,搜索了好久也没能整明白卡尔曼滤波里的几个变量都具体起什么作用。
稀里糊涂的算有效果了。
国内网站大家都是你转载我的,我转载你的,没几个说的明白的,更有甚者,转载都出现错误。
没办法,试试去外网找,还真找着好用的了。呵呵。贴出来大家参考下。

/** A simple kalman filter example by Adrian Boeing
www.adrianboeing.com
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

double frand() {
    return 2*((rand()/(double)RAND_MAX) - 0.5);
}

int main() {

    //initial values for the kalman filter
    float x_est_last = 0;
    float P_last = 0;
    //the noise in the system
    float Q = 0.022;
    float R = 0.617;
   
    float K;
    float P;
    float P_temp;
    float x_temp_est;
    float x_est;
    float z_measured; //the 'noisy' value we measured
    float z_real = 0.5; //the ideal value we wish to measure
   
    srand(0);
   
    //initialize with a measurement
    x_est_last = z_real + frand()*0.09;
   
    float sum_error_kalman = 0;
    float sum_error_measure = 0;
   
    for (int i=0;i<30;i++) {
        //do a prediction
        x_temp_est = x_est_last;
        P_temp = P_last + Q;
        //calculate the Kalman gain
        K = P_temp * (1.0/(P_temp + R));
        //measure
        z_measured = z_real + frand()*0.09; //the real measurement plus noise
        //correct
        x_est = x_temp_est + K * (z_measured - x_temp_est);
        P = (1- K) * P_temp;
        //we have our new system
        
        printf("Ideal    position: %6.3f \n",z_real);
        printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));
        printf("Kalman   position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est));
        
        sum_error_kalman += fabs(z_real - x_est);
        sum_error_measure += fabs(z_real-z_measured);
        
        //update our last's
        P_last = P;
        x_est_last = x_est;
    }
   
    printf("Total error if using raw measured:  %f\n",sum_error_measure);
    printf("Total error if using kalman filter: %f\n",sum_error_kalman);
    printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100));
   
   
    return 0;
}//在此未对原程序做任何改动

高手指点一下 Q 和 R 这两个噪声量是干嘛的吧!它们的大小会影响什么?

阿莫论坛20周年了!感谢大家的支持与爱护!!

一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。

出0入0汤圆

 楼主| 发表于 2011-8-21 12:29:45 | 显示全部楼层
回复【5楼】cbeeee  
-----------------------------------------------------------------------

恩,想想确实是这样。

谢谢!

出0入0汤圆

 楼主| 发表于 2011-8-21 12:30:29 | 显示全部楼层
回复【6楼】lijieamd  
-----------------------------------------------------------------------

有道理,研究挺深啊。

出0入0汤圆

 楼主| 发表于 2011-8-21 12:35:01 | 显示全部楼层
回复【9楼】02015678 周光
-----------------------------------------------------------------------

确实不错!谢谢!
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-3-29 23:54

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表