/**
* 卡尔曼的初始化信息
*/
class KalmanInfo {
var mP =1.0 //后验状态估计值误差的方差的初始值（不要为0问题不大）
var mQ =1.0 //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
var mR =100.0 //测量（观测）噪声方差 可以通过实验手段获得
var filterValue =0.0// 测量的初始值
var kalmanGain =0.0
}
class KalmanFilterUtils {
/**Kalman Filter */
fun KalmanAlgorithm(kalmanInfo: KalmanInfo, lastMeasurement:Double):KalmanInfo
{
//预测下一时刻的值
//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
}
}
第一次的估计值就是真实值
