|
|
卡爾曼濾波算法
所有代碼下載：
卡爾曼濾波代碼.zip
(2.89 KB, 下載次數: 114)
2016-7-23 13:08 上傳
點擊文件名下載附件
使用方法：
#include "kalman.h"
#include "stdio.h"
#include "stdlib.h"
void main(void)
{
KalmanCountData k;
//定義一個卡爾曼運算結構體
Kalman_Filter_Init(&k);
//講運算變量初始化
int m,n;
for(int a = 0;a<80;a++)
//測試80次
{
//m,n為1到100的隨機數
m = 1+ rand() %100;
n = 1+ rand() %100;
//卡爾曼濾波，傳遞2個測量值以及運算結構體
Kalman_Filter((float)m,(float)n,&k);
//打印結果
printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0);
}
}
- /**
- ******************************************************************************
- * @file kalman.h
- * @author willieon
- * @version V0.1
- * @date January-2015
- * @brief 卡爾曼濾波算法
- *
- *
- ******************************************************************************
- * @attention
- *本人對卡爾曼的粗略理解：以本次測量角速度（陀螺儀測量值）的積分得出的角度值
- * 與上次最優角度值的方差產生一個權重來衡量本次測量角度（加速度測量值）
- * 與上次最優角度值，從而產生新的最優角度值。好吧，比較拗口，有誤處忘指正。
- *
- ******************************************************************************
- */
- #ifndef __KALMAN_H__
- #define __KALMAN_H__
- #define Q_angle 0.001 ////角度過程噪聲的協方差
- #define Q_gyro 0.003 ////角速度過程噪聲的協方差
- #define R_angle 0.5 ////測量噪聲的協方差（即是測量偏差）
- #define dt 0.01 ////卡爾曼濾波采樣頻率
- #define C_0 1
- /**************卡爾曼運算變量定義**********************
- *
- ***由于卡爾曼為遞推運算，結構體需定義為全局變量
- ***在實際運用中只需定義一個KalmanCountData類型的變量即可
- ***無需用戶定義多個中間變量，簡化函數的使用
- */
- typedef struct
- {
- float Q_bias; ////最優估計值的偏差，即估計出來的陀螺儀的漂移量
- float Angle_err; ////實測角度與陀螺儀積分角度的差值
- float PCt_0;
- float PCt_1;
- float E; ////計算的過程量
- float K_0; ////含有卡爾曼增益的另外一個函數，用于計算最優估計值
- float K_1; ////含有卡爾曼增益的函數，用于計算最優估計值的偏差
- float t_0;
- float t_1;
- float Pdot[4]; ////Pdot[4] = {0,0,0,0};過程協方差矩陣的微分矩陣
- float PP[2][2]; //// PP[2][2] = { { 1, 0 },{ 0, 1 } };協方差(covariance)
- float Angle_Final; ////后驗估計最優角度值（即系統處理最終值）
- float Gyro_Final; ////后驗估計最優角速度值
- }KalmanCountData;
- void Kalman_Filter(float Accel, float Gyro ,KalmanCountData * Kalman_Struct);
- void Kalman_Filter_Init(KalmanCountData * Kalman_Struct);
- #endif
|
評分
-
查看全部評分
|
[復制鏈接]
