搜索
bottom↓
回复: 3

卡尔曼滤波程序问题请教

[复制链接]

出0入0汤圆

发表于 2017-5-26 17:19:33 | 显示全部楼层 |阅读模式
以前在论坛里找到了一段卡尔曼滤波算法程序,我做了一下测试结果很不理想,
我使用AD采集的是数转换成浮点然后经过这个滤波程序,但是输出结果是100恒定不变,
不知道是什么原因请大家帮忙分析一下,下面是代码

#define KALMAN_Q        0.0000001
#define KALMAN_R        0.0001
#define KALMAN_P0        -0.00001
/*       
        Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
        R:测量噪声,R增大,动态响应变慢,收敛稳定性变好       
*/

double KalmanFilter(const double ResrcData, double ProcessNiose_Q, double MeasureNoise_R, double InitialPrediction)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;

        static        double x_last;

        double x_mid = x_last;
        double x_now;

        static        double p_last;

        double p_mid ;
        double p_now;
        double kg;       

        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为kalman filter,R为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
               
        p_now=(1-kg)*p_mid;//最优值对应的covariance       

        p_last = p_now; //更新covariance值
        x_last = x_now; //更新系统状态值

        return x_now;               
}

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

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

出140入115汤圆

发表于 2017-5-28 14:13:40 | 显示全部楼层
论坛帖子:
卡尔曼滤波算法C语言实现(转2)

非常感谢原作者,我在这个的基础上转换成纯整形运算。STM32F103 12位ADC先放大1000倍再运算,理论上可以保留小数点后三位的结果。
效果非常不错,运算速度也快,72M时钟 1-2uS左右(根据MDK周期数)。
  1. uint32_t KalmanFilter(int32_t ResrcData)
  2. {
  3.     /*-------------------------------------------------------------------------------------------------------------*/
  4.     /*
  5.             Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
  6.             R:测量噪声,R增大,动态响应变慢,收敛稳定性变好
  7.     */
  8.     /*-------------------------------------------------------------------------------------------------------------*/
  9.     static int32_t R = (int32_t)(128*1024);
  10.     static int32_t Q = (int32_t)4;
  11.     static uint32_t Counter1 = 0;
  12.     static uint32_t Counter2 = 0;
  13.     static int32_t x_last = 0;
  14.         static int32_t p_last;   // 应赋初始估计值
  15.     int32_t x_mid;
  16.     int32_t x_now;
  17.     int32_t p_mid ;
  18.     int32_t p_now;

  19.     ResrcData *= 1024;
  20.     x_now = ResrcData - x_last;
  21.     if(x_now < 0)
  22.     {
  23.         x_now *= -1; // 取绝对值
  24.     }
  25.     if(x_now >= 32*1024)   // 如果测量值连续比估计值大或小 相信测量值,加速迭代
  26.     {
  27.         Counter1++;
  28.         Counter2 = 0;
  29.         if(Counter1 > 10)
  30.         {
  31.             R = 512;;
  32.             Q = 128;
  33.         }
  34.     }
  35.     else                 // 数据比较稳定,加强滤波
  36.     {
  37.         Counter1 = 0;
  38.         Counter2++;
  39.         if(Counter2 > 10)  
  40.         {
  41.             R = (int32_t)(128*1024);
  42.             Q = (int32_t)4;
  43.         }
  44.     }
  45.     x_mid = x_last;   // x_last=x(k-1|k-1),x_mid=x(k|k-1)
  46.     p_mid = p_last + Q; // p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
  47. //    kg = p_mid/(p_mid + R); //kg为kalman filter,R为噪声
  48. //    x_now = x_mid+kg*(ResrcData - x_mid);// 估计出的最优值
  49.     x_now = x_mid + (p_mid*(ResrcData - x_mid))/(p_mid + R);
  50. //    p_now = (1 - kg)*p_mid; // 最优值对应的covariance
  51.     p_now = p_mid - p_mid*p_mid/(p_mid + R); // 最优值对应的covariance
  52.     p_last = p_now;  // 更新covariance值
  53.     x_last = x_now;  // 更新系统状态值
  54.     x_now /= 1024;
  55.     if((x_now > 4096)||( x_now < 0))
  56.     {
  57.         x_last = ResrcData;
  58.         p_now = ResrcData;
  59.         x_now = ResrcData/1024;
  60.     }
  61.     return (u32)x_now;
  62. }
复制代码


出0入0汤圆

发表于 2018-7-1 07:51:56 | 显示全部楼层
感谢楼主,受教了

出0入0汤圆

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

本版积分规则

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

GMT+8, 2024-3-29 01:02

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

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