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