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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 1979|回復: 1
收起左側(cè)

關于卡爾曼濾波的C語言程序

[復制鏈接]
ID:590486 發(fā)表于 2019-8-2 21:56 | 顯示全部樓層 |閱讀模式
關于卡爾曼濾波的C程序

單片機源程序如下:
  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;    //測量噪聲的協(xié)方差 既測量偏差
  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-先驗估計誤差協(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-先驗估計誤差協(xié)方差微分的積分
  33.         PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協(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;                 //后驗估計誤差協(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. }
復制代碼

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




回復

使用道具 舉報

ID:693716 發(fā)表于 2020-2-19 12:32 來自觸屏版 | 顯示全部樓層
謝謝分享，正學習呢
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網(wǎng)

快速回復 返回頂部 返回列表