|
|
卡爾曼濾波算法，自己，輸入為隨機數(shù)，Q和R要自己選
主函數(shù)：
#include "kalman.h"
#include "stdio.h"
#include "stdlib.h"
void main(void)
{
KalmanCountData k;
//定義一個卡爾曼運算結(jié)構(gòu)體
Kalman_Filter_Init(&k);
//講運算變量初始化
int n;
int a;
srand((unsigned)time(NULL)); // 初始化隨機種子，使rand()產(chǎn)生的隨機數(shù)每次不一樣
for(a = 0;a<1000;a++)
//測試1000次
{
//m,n為1到100的隨機數(shù)
n = 1+ rand() %100;
//卡爾曼濾波，傳遞2個測量值以及運算結(jié)構(gòu)體
Kalman_Filter(&k,n);
//打印結(jié)果
printf("第%d次迭代\n 輸入：%d, 輸出濾波值為：%f\r\n",a+1,n,k.Filter_Value);
}
}
kalman.h:
typedef struct //定義結(jié)構(gòu)體
{
float Filter_Value; //K-1時刻的系濾波值
float A; //系統(tǒng)參數(shù)，一維模型中通常選為1
float H; //系統(tǒng)測量參數(shù)，一維模型選擇為1
float Kg; //卡爾曼增益
float Q; //過程噪聲偏差
float R; //測量噪聲偏差
float P; //估計誤差協(xié)方差
} KalmanCountData;
void Kalman_Filter_Init(KalmanCountData * Kalman_Struct) //卡爾曼濾波器初始化
{
Kalman_Struct->A = 1;
Kalman_Struct->H = 1;
Kalman_Struct->P = 3; //后驗狀態(tài)估計值方差的初始值
Kalman_Struct->Q = 0.1; //過程噪聲偏差，實驗測的 ，要填的！
Kalman_Struct->R = 10; //測量噪聲偏差，要填的！
Kalman_Struct->Filter_Value = 0; //初始化設(shè)置k-1時刻的系統(tǒng)濾波后的值為0
}
double Kalman_Filter(KalmanCountData* Kalman_Struct, double Last_Measurement){
double PredictValue = Kalman_Struct->Filter_Value*Kalman_Struct->A; //先驗估計的預(yù)測值
Kalman_Struct->P = Kalman_Struct->A*Kalman_Struct->P*Kalman_Struct->A+Kalman_Struct->Q; //求協(xié)方差
double prevalve = Kalman_Struct->Filter_Value; //記錄上一次（k-1）的值
Kalman_Struct->Kg = Kalman_Struct->P*Kalman_Struct->H/(Kalman_Struct->H*Kalman_Struct->P*Kalman_Struct->H + Kalman_Struct->R);
/*
卡爾曼增益Kg = P(k|k-1) H’ / (H P(k|k-1) H’ + R)
*/
Kalman_Struct->Filter_Value = PredictValue + (Last_Measurement-PredictValue) * Kalman_Struct->Kg;
/*
修正結(jié)果，更新預(yù)測值；X(k|k) = X(k|k-1) + (Z(k) - H* X(k|k-1)) * Kg
*/
Kalman_Struct->P = (1 - Kalman_Struct->Kg * Kalman_Struct->H) * Kalman_Struct->P;
//更新后驗估計的協(xié)方差，P(k|k) = (1 - Kg * H) * P(k|k-1)
return Kalman_Struct->Filter_Value;
}
|
-
-
Kalman.zip
2019-1-7 17:00 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
89.46 KB, 下載次數(shù): 38, 下載積分: 黑幣 -5
評分
-
查看全部評分
|
