卡爾曼濾波C語(yǔ)言,二維

1、卡爾曼濾波(C語(yǔ)言,二維)卡爾曼濾波,C語(yǔ)言,二維/*titlekalmanfilter;*brief 一維卡爾曼濾波器*timeXX 年.10.19*editor 小南&zin*飛控愛(ài)好 QQ 群 551883670郵箱 759421287*/#include “kalman.h #include#include sys.h /*卡爾曼濾波*brief:線(xiàn)性最優(yōu)評(píng)估濾波*paraminInputData濾波前的數(shù)據(jù),QR誤差*paramoutNone*return濾波后的數(shù)據(jù)*remark:通過(guò)修改正程噪聲和測(cè)量噪聲R,Q值優(yōu)化輸出值X(k)=AX(k-1)+BU(k)+W(k)Z(k尸HX

2、(k)+V(k)AB是系統(tǒng)參數(shù)X K時(shí)刻的系統(tǒng)狀態(tài)H:測(cè)量系統(tǒng)的參數(shù)Z:K時(shí)刻的測(cè)量值WV:過(guò)程和測(cè)量噪聲X(k|k-1)=X(k-1|k-1)預(yù)測(cè)下一狀態(tài)的系統(tǒng)P(k|k-1)=P(k-1|k-1)+QKg(k)=P(k|k-1)/(P(k|k-1)+R)計(jì)算 Kg 卡爾曼增益X(k|k)=X(k|k-1)+Kg(k)(Z(k)-X(k|k-1)根據(jù)預(yù)測(cè)值結(jié)合估算值得出當(dāng)前狀態(tài)值P(k|k)=(1-Kg(k)P(k|k-1)更新協(xié)方差(k-1|k-1)上一個(gè)狀態(tài)的最優(yōu)評(píng)估(k|k-1)由上一個(gè)狀態(tài)的最優(yōu)評(píng)估預(yù)測(cè)當(dāng)前狀態(tài)的最優(yōu)評(píng)估(k|k)由預(yù)測(cè)本狀態(tài)的評(píng)估具體實(shí)現(xiàn)最優(yōu)評(píng)估Q:系統(tǒng)過(guò)程的協(xié)方差

3、R:測(cè)量的協(xié)方差高斯白=Q+RKg:卡爾曼增益P:協(xié)方差強(qiáng)調(diào):調(diào)整卡爾曼的濾波效果時(shí)只要調(diào)整過(guò)程噪聲和測(cè)量噪聲即可,初始X可0,P取小值強(qiáng)調(diào):調(diào)整卡爾曼的濾波效果時(shí)只要調(diào)整過(guò)程噪聲和測(cè)量噪聲即可,初始X可0,P取小值強(qiáng)調(diào):調(diào)整卡爾曼的濾波效果時(shí)只要調(diào)整過(guò)程噪聲和測(cè)量噪聲即可,初始X可0,P取小值強(qiáng)調(diào):調(diào)整卡爾曼的濾波效果時(shí)只要調(diào)整過(guò)程噪聲和測(cè)量噪聲即可,初始X可0,P取小值由于X是初始狀態(tài),也就是進(jìn)入測(cè)量前的數(shù)據(jù),任意設(shè)定.初始時(shí)候還未進(jìn)入 測(cè)量的時(shí)候的數(shù)值X不認(rèn)為是方差為零的理想數(shù)據(jù).*/調(diào)整卡爾曼的濾波效果時(shí)只要調(diào)整過(guò)程噪聲和測(cè)量噪聲即可/卡爾曼濾波屬于專(zhuān)門(mén)針對(duì)高斯白的濾波器,無(wú)法應(yīng)對(duì)突變干擾,所以加一個(gè) 除去最大最小值的滑動(dòng)均值濾波./多維卡爾曼是多維噪聲的擴(kuò)展,如在飛行器的系統(tǒng)中,一維是加速度的誤差 二維是加速度和角速度兩個(gè)維度的誤差,/三維那么是角速度,加