找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

帖子
查看: 1775|回復(fù): 1
收起左側(cè)

關(guān)于卡爾曼濾波的C語言程序

[復(fù)制鏈接]
ID:590486 發(fā)表于 2019-8-2 21:56 | 顯示全部樓層 |閱讀模式
關(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;    //測量噪聲的協(xié)方差 既測量偏差
  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)




回復(fù)

使用道具 舉報(bào)

ID:693716 發(fā)表于 2020-2-19 12:32 來自手機(jī) | 顯示全部樓層
謝謝分享，正學(xué)習(xí)呢
回復(fù)

使用道具 舉報(bào)

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

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

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

快速回復(fù) 返回頂部 返回列表