/**
* 卡爾曼的初始化信息
*/
class KalmanInfo {
var mP =1.0? //后驗狀態(tài)估計值誤差的方差的初始值（不要為0問題不大）
? ? var mQ =1.0? //預(yù)測（過程）噪聲方差 影響收斂速率，可以根據(jù)實際需求給出
? ? var mR =100.0? ? //測量（觀測）噪聲方差 可以通過實驗手段獲得
? ? var filterValue =0.0// 測量的初始值
? ? var kalmanGain =0.0
}
class KalmanFilterUtils {
/**Kalman Filter */
? ? fun KalmanAlgorithm(kalmanInfo: KalmanInfo, lastMeasurement:Double):KalmanInfo
{
//預(yù)測下一時刻的值
//val predictValue:Double = kalmanInfo.mA*kalmanInfo.filterValue
? ? ? ? kalmanInfo.mP = kalmanInfo.mP+kalmanInfo.mQ
? ? ? // val preValue = kalmanInfo.filterValue
? ? ? ? kalmanInfo.kalmanGain = kalmanInfo.mP/(kalmanInfo.mP + kalmanInfo.mR)
kalmanInfo.filterValue =? kalmanInfo.filterValue +(lastMeasurement- kalmanInfo.filterValue)* kalmanInfo.kalmanGain
? ? ? ? kalmanInfo.mP = (1 - kalmanInfo.kalmanGain)*kalmanInfo.mP
? ? ? ? return kalmanInfo
}
}
第一次的估計值就是真實值
