標(biāo)題: 好不容易找的卡爾曼濾波的代碼，分享給大家 [打印本頁]
作者: happyfun    時間: 2016-7-23 13:09
標(biāo)題: 好不容易找的卡爾曼濾波的代碼，分享給大家
卡爾曼濾波算法

所有代碼下載：
卡爾曼濾波代碼.zip (2.89 KB, 下載次數(shù): 114)
使用方法：
#include "kalman.h"

#include "stdio.h"

#include "stdlib.h"






void main(void)
{


    KalmanCountData k;
    //定義一個卡爾曼運算結(jié)構(gòu)體
    Kalman_Filter_Init(&k);
    //講運算變量初始化
    int m,n;        

       for(int a = 0;a<80;a++)
    //測試80次
    {

        //m,n為1到100的隨機數(shù)
        m = 1+ rand() %100;

        n = 1+ rand() %100;

                //卡爾曼濾波，傳遞2個測量值以及運算結(jié)構(gòu)體
   
    Kalman_Filter((float)m,(float)n,&k);

        //打印結(jié)果
        printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0);
   
    }




}

  1. /**
  2.   ******************************************************************************
  3.   * @file    kalman.h
  4.   * @author  willieon
  5.   * @version V0.1
  6.   * @date    January-2015
  7.   * @brief   卡爾曼濾波算法
  8.   *        
  9.   *
  10.   ******************************************************************************
  11.   * @attention
  12.   *本人對卡爾曼的粗略理解：以本次測量角速度（陀螺儀測量值）的積分得出的角度值
  13.   * 與上次最優(yōu)角度值的方差產(chǎn)生一個權(quán)重來衡量本次測量角度（加速度測量值）
  14.   * 與上次最優(yōu)角度值，從而產(chǎn)生新的最優(yōu)角度值。好吧，比較拗口，有誤處忘指正。
  15.   *
  16.   ******************************************************************************
  17.   */

  19. #ifndef __KALMAN_H__
  20. #define __KALMAN_H__


  23. #define Q_angle                        0.001                                ////角度過程噪聲的協(xié)方差
  24. #define Q_gyro                        0.003                                ////角速度過程噪聲的協(xié)方差
  25. #define R_angle                        0.5                                        ////測量噪聲的協(xié)方差（即是測量偏差）
  26. #define dt                                0.01                                ////卡爾曼濾波采樣頻率
  27. #define C_0                                1

  29. /**************卡爾曼運算變量定義**********************
  30. *
  31. ***由于卡爾曼為遞推運算，結(jié)構(gòu)體需定義為全局變量
  32. ***在實際運用中只需定義一個KalmanCountData類型的變量即可
  33. ***無需用戶定義多個中間變量，簡化函數(shù)的使用
  34. */
  35. typedef struct
  36. {
  37.         float                                Q_bias;                                ////最優(yōu)估計值的偏差，即估計出來的陀螺儀的漂移量
  38.         float                                Angle_err;                        ////實測角度與陀螺儀積分角度的差值
  39.         float                                PCt_0;                                
  40.         float                                PCt_1;
  41.         float                                E;                                        ////計算的過程量
  42.         float                                K_0;                                ////含有卡爾曼增益的另外一個函數(shù)，用于計算最優(yōu)估計值
  43.         float                                K_1;                                ////含有卡爾曼增益的函數(shù)，用于計算最優(yōu)估計值的偏差
  44.         float                                t_0;                                
  45.         float                                t_1;
  46.         float                                Pdot[4];                         ////Pdot[4] = {0,0,0,0};過程協(xié)方差矩陣的微分矩陣
  47.         float                                PP[2][2];                         //// PP[2][2] = { { 1, 0 },{ 0, 1 } };協(xié)方差(covariance)
  48.         float                                Angle_Final;                 ////后驗估計最優(yōu)角度值（即系統(tǒng)處理最終值）
  49.         float                                Gyro_Final;                         ////后驗估計最優(yōu)角速度值

  51. }KalmanCountData;

  53. void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct);
  54. void Kalman_Filter_Init(KalmanCountData * Kalman_Struct);



  58. #endif
復(fù)制代碼


作者: lei787654    時間: 2017-9-26 12:16
看看，謝謝樓主
作者: 大豬蹄子    時間: 2019-1-17 15:43
謝謝樓主，下載了，先看看羅
作者: 夢園心田    時間: 2019-1-21 01:49
lei787654 發(fā)表于 2017-9-26 12:16
**** 作者被禁止或刪除 內(nèi)容自動屏蔽 ****

很好的東西，以后可能用得上
作者: jifengjianwu    時間: 2019-1-21 09:03
網(wǎng)上入門級的資料很多，哪里能找到稍微深入些的資料呢？
作者: wangxuewei    時間: 2021-1-7 14:16
共師兄習(xí)，共同進步
作者: 42151    時間: 2021-4-7 15:33
請問為何要調(diào)入角速度值？



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