标题: 关于卡尔曼滤波的C语言程序 [打印本页]
作者: 我是新手啊126    时间: 2019-8-2 21:56
标题: 关于卡尔曼滤波的C语言程序
关于卡尔曼滤波的C程序

单片机源程序如下:
  1. #include "Kalman_Filter.h"
  2. /******************************
  3. 函数名称：Kalman_Filter
  4. 输入参数：x（需要优化的数据）、v（x的变化率）
  5. 返回参数：x_youhua（优化后的x数据）
  6. 其中Q_x为x的置信度，Q_v为v的置信度；

  8. ********************************/
  9. float Kalman_Filter(float x,float v)               
  10. {
  11.         static float v_sum;   //v数据暂存
  12.         static float x_youhua = 0.0;
  13.         static float Q_bias = 0.0;
  14.         static float angle_err = 0.0;
  15.         static float Q_x = 0.1;  //x数据置信度
  16.         static float Q_v = 0.1;   //v数据置信度
  17.         static float R_x = 0.5;    //测量噪声的协方差 既测量偏差
  18.         static float dt = 0.005;           //dt为滤波器采样时间(秒)
  19.         static char  C_0 = 1;
  20.         static float PCt_0=0, PCt_1=0, E=0;
  21.         static float K_0=0, K_1=0, t_0=0, t_1=0;
  22.         static float Pdot[4] ={0,0,0,0};
  23.         static float PP[2][2] = { { 1, 0 },{ 0, 1 } };

  25.         x_youhua += (v - Q_bias) * dt; //先验估计
  26.         angle_err = x - x_youhua;        //zk-先验估计

  28.         Pdot[0]=Q_x - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
  29.         Pdot[1]=- PP[1][1];
  30.         Pdot[2]=- PP[1][1];
  31.         Pdot[3]=Q_v;
  32.         PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
  33.         PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
  34.         PP[1][0] += Pdot[2] * dt;
  35.         PP[1][1] += Pdot[3] * dt;       

  37.         PCt_0 = C_0 * PP[0][0];
  38.         PCt_1 = C_0 * PP[1][0];
  39.        
  40.         E = R_x + C_0 * PCt_0;
  41.        
  42.         K_0 = PCt_0 / E;
  43.         K_1 = PCt_1 / E;
  44.        
  45.         t_0 = PCt_0;
  46.         t_1 = C_0 * PP[0][1];

  48.         PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
  49.         PP[0][1] -= K_0 * t_1;
  50.         PP[1][0] -= K_1 * t_0;
  51.         PP[1][1] -= K_1 * t_1;
  52.                
  53.         x_youhua += K_0 * angle_err;         //最优x
  54.         Q_bias += K_1 * angle_err;         
  55.         v_sum = v - Q_bias;         //最优v

  57.         return (x_youhua);                                 //最终x                 
  58. }
复制代码

所有资料51hei提供下载:
卡尔曼滤波模块.zip (1.14 KB, 下载次数: 51)




作者: hj08102    时间: 2020-2-19 12:32
谢谢分享，正学习呢



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1