欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標(biāo)題: 關(guān)于卡爾曼濾波的C語言程序 [打印本頁]
作者: 我是新手啊126    時(shí)間: 2019-8-2 21:56
標(biāo)題: 關(guān)于卡爾曼濾波的C語言程序
關(guān)于卡爾曼濾波的C程序

單片機(jī)源程序如下:
  1. #include "Kalman_Filter.h"
  2. /******************************
  3. 函數(shù)名稱：Kalman_Filter
  4. 輸入?yún)?shù)：x（需要優(yōu)化的數(shù)據(jù)）、v（x的變化率）
  5. 返回參數(shù)：x_youhua（優(yōu)化后的x數(shù)據(jù)）
  6. 其中Q_x為x的置信度，Q_v為v的置信度；

  8. ********************************/
  9. float Kalman_Filter(float x,float v)               
  10. {
  11.         static float v_sum;   //v數(shù)據(jù)暫存
  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數(shù)據(jù)置信度
  16.         static float Q_v = 0.1;   //v數(shù)據(jù)置信度
  17.         static float R_x = 0.5;    //測(cè)量噪聲的協(xié)方差 既測(cè)量偏差
  18.         static float dt = 0.005;           //dt為濾波器采樣時(shí)間(秒)
  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; //先驗(yàn)估計(jì)
  26.         angle_err = x - x_youhua;        //zk-先驗(yàn)估計(jì)

  28.         Pdot[0]=Q_x - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分
  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-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
  33.         PP[0][1] += Pdot[1] * dt;   // =先驗(yàn)估計(jì)誤差協(xié)方差
  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;                 //后驗(yàn)估計(jì)誤差協(xié)方差
  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;         //最優(yōu)x
  54.         Q_bias += K_1 * angle_err;         
  55.         v_sum = v - Q_bias;         //最優(yōu)v

  57.         return (x_youhua);                                 //最終x                 
  58. }
復(fù)制代碼

所有資料51hei提供下載:
卡爾曼濾波模塊.zip (1.14 KB, 下載次數(shù): 51)




作者: hj08102    時(shí)間: 2020-2-19 12:32
謝謝分享，正學(xué)習(xí)呢



歡迎光臨 (http://www.raoushi.com/bbs/) Powered by Discuz! X3.1