java 卡尔曼滤波算法,java版kalman滤波算法

/**

* 卡尔曼的初始化信息

*/

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

}

}

第一次的估计值就是真实值