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

}

}

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

©著作权归作者所有,转载或内容合作请联系作者
平台声明：文章内容（如有图片或视频亦包括在内）由作者上传并发布，文章内容仅代表作者本人观点，简书系信息发布平台，仅提供信息存储服务。

推荐阅读更多精彩内容