#include?“kalman.h“
#include?
#include?
#include?
#include?

Kalman::Kalman（）
{
????m_dQ?=?0.0;
????m_dR?=?0.0;
????//?初始化容器大小
????m_vecSysNoise.resize（N）;
????m_vecObserNoise.resize（N）;
????m_vecReal.resize（N）;
????m_vecObser.resize（N）;
????m_vecKF.resize（N）;
????m_vecCov.resize（N）;
}

void?Kalman::setIniVal（float?dval?float?dQ?float?dR）
{
????m_dQ?=?dQ;
????m_dR?=?dR;
????m_vecReal[0]?=?dval;
????m_vecObser[0]?=?dval;
????m_vecKF[0]?=?dval;

????//?初始化系統噪聲
????for（int?i?=?0;?i?????{
????????m_vecSysNoise[i]?=?sqrt（m_dQ）?*?frand（）;
????}

????//?初始化觀測噪聲
????for（int?i?=?0;?i?????{
????????m_vecObserNoise[i]?=?sqrt（m_dR）?*?frand（）;
????}

????//?協方差賦初值
????m_vecCov[1]?=?0.01;
}

void?Kalman::getData（vector?&vecReal
?????????????????????vector?&vecObserver
?????????????????????vector?&vecFilter）
{
????float?dXPre?=?0.0;???//?一步預測值
????float?dPpre?=?0.0;???//?協方差一步預測
????float?Kg?=?0.0;??????//?濾波增益

????for（int?i?=?1;?i?????{
????????m_vecReal[i]?=?m_vecReal[i-1]?+?m_vecSysNoise[i-1];?//?真實溫度波動變化
????????m_vecObser[i]?=?m_vecReal[i]?+?m_vecObserNoise[i];??//?觀測值波動變化
????????//?以下五步為Kalman核心步驟
????????dXPre?=?m_vecKF[i-1];?//?一步預測
????????dPpre?=?m_vecCov[i-1]?+?m_dQ;??//?協方差一步預測
????????Kg?=?dPpre?/?（dPpre?+?m_dR）;???//?計算增益
????????m_vecKF[i]?=?dXPre?+?Kg?*?（m_vecObser[i]?-?dXPre）;?//?狀態更新
????????m_vecCov[i]?=?（1?-?Kg）?*?dPpre;??//?協方差更新
????}
????//?輸出結果
????vecReal?=?m_vecReal;
????vecObserver?=?m_vecObser;
????vecFilter?=?m_vecKF;
????return;
}

void?Kalman::displayerror（）
{
????float?ObError?=?0.0;???//?觀測誤差
????float?KfError?=?0.0;???//?卡爾曼濾波誤差

????for（int?i?=?0;?i?????{
????????cout?<<“?Real?Value?“<????????cout?<<“?Observer?Value?“<????????cout?<<“?Error?“<????????cout?<<“?KF?Value?“<????????cout?<<“?Error?“<????????ObError?+=?fabs（m_vecReal[i]?-?m_vecObser[i]）;
????????KfError?+=?fabs（m_vecReal[i]?-?m_vecKF[i]）;
????}

????cout<<“\n“<<“?Observer?Error?“<????cout<<“?KalmanFilter?Error?“<}



float?Kalman::frand（）
{
????static?int?seed?=?0;
????int?i?=?time（0）?%?100000;
????seed?+=?i;
????srand（seed）;
????float?a?=??2?*?（（rand（）?/?（float）RAND_MAX）?-?0.5）;//隨機噪聲
????return?a;
}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????2774??2019-06-29?15:59??kalman.cpp
?????文件????????1261??2019-06-29?16:01??kalman.h
?????文件?????????288??2019-06-29?15:53??main.cpp
?????文件?????????148??2019-06-28?21:11??KalmanFilter.pro