卡爾曼濾波在單片機上的使用

}}卡爾曼濾波在單片機上的使用2011-04-1313:43:43|分類：默認分類|標簽：|字號大中小訂閱卡爾曼增益：KalmanGain縮寫KG本次估計值：EstiniateValueNow縮寫EVN上次估計值：EstimateValuePre縮寫EVP當前估計協(xié)方差：EstImateCovarianceNOW縮寫ECN下次估計協(xié)方差：EstImateCovarianceNext縮寫ECX測量值：MeasureValue縮寫MV當麗測量協(xié)方差：MeasureCovarianceXow縮寫MCN下次測量協(xié)方差：MeasureCovarianct縮寫MCX估「變化比：E航Im&teChaiigERd縮寫ECREVN二ECR^EVP^KGx(MV-EVP)(公式1)笫一次獲得了測量值MV(0)第一步，讓算KG(0),利用(公式2)第二步，有了KG⑹,我們就可以計算當血估計值EV\(0)了，利用&第三步，計算出了E￥N(0)?那么本次濾波的任務(wù)已經(jīng)完成一半了，我要完成下一次測量所需要遞歸值就可以了，所以「EVP(O=EVN(0)o第四步.利用(公式3)(公式4)計算出ECX和\【CX,所以ECX(1)=ECX,MCN(1)=MCX.到此，本次濾波完成.其中EVX(X)序列就是我們的濾波輸出序列「#ifndef_KALMAN_H_#define_KALMAN_H_externKalmanGain;//卡爾曼增益externEstimateCovariance;//估計協(xié)方差externMeasureCovariance;//測量協(xié)方差externEstimateValue;//估計值externvoidKalmanFilterInit(void);externKalmanFilter(Measure);#endif#include"config.h"#include"math.h"KalmanGain;//卡爾曼增益EstimateCovariance;//估計協(xié)方差MeasureCovariance;//測量協(xié)方差EstimateValue;//估計值voidKalmanFilterInit(void);externfloatKalmanFilter(floatMeasure);voidKalmanFilterInit(void){EstimateValue=0;EstimateCovariance=0.1;MeasureCovariance=0.02;KalmanFilter(Measure){//計算卡爾曼增益KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance))；//計算本次濾波估計值EstimateValue=EstimateValue+KalmanGain*(Measure—EstimateValue)；//更新估計協(xié)方差EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance;//更新測量方差MeasureCovari