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

}

}

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