kalman卡尔曼算法JS简单描述

先上代码

/**
 * 
 * @param {
 *  lastVal 上次的值
 *  predict 预测函数
 *  error 误差
 *  predictError 预测误差
 *  measureError 测量误差
 *  measure 测量值
 * } args 
 */
let kalman = (args)=>{

    //预测值=>根据上个时刻的最优值预测
    let predict = args.predict(args.lastVal);  

    //新的误差=》 上次算的最优误差值平方 + 预测误差平方（指定的） 再开根号
    let error = Math.sqrt(Math.pow(args.nError, 2) + Math.pow(args.predictError, 2));
    //console.log("error:", error);
    //计算卡尔曼增益 新的误差平方 /  （新的误差平方 + 观测值误差的平方）
    let gain = Math.sqrt(Math.pow(error, 2) / (Math.pow(error, 2) + Math.pow(args.measureError, 2)));
    //console.log("gain:", gain);
    //计算最优值
    let value = predict + gain * (args.measure - predict);
    console.log("value", value);
    //为下一次计算误差值
    let nError = Math.sqrt((1 - gain) * Math.pow(error, 2));

    //更新参数
    args.lastVal = value;
    args.nError = nError;
};

module.exports = {
    kalman
};

测试代码

//const sp = require('serialport')

//console.log(sp)

const k = require('./kalman');

let data = [1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 9, 8, 8, 7, 5];

let args = {
    lastVal: 0,
    predict: (lastVal) => {
        return lastVal + 1;
    },
    nError: 0,
    predictError: 0.5,
    measureError: 0.1,
    measure: 0.95
};

data.forEach( (val, index)=>{
    let e =  (Math.random() - 0.5) / 5;
    
    console.log("step " + index + "-----------:");
    console.log("ref:", val);
    console.log("me:", e);

    args.measure = val + e;
    k.kalman(args);
    //console.log("step " + index + ":", args);
});

更新
后面再理解了一下卡尔曼。
预测方程：
当前预测值 = f(上个最佳估计)
f()是一个函数，对于线性的而言 f(x) = ax1 + b，b代表了预测中的误差方差

量测方程：
测量值 = f(当前估计值，即量测方程中的因变量)
f()是一个函数，对于线性的而言 f(x) = a1x + r，r代表了测量中的误差方差

预测误差方差:
我们还需要对误差进行预测， pk = apka + b ,即上个预测的误差 + 当前预测的误差 ，在初始化时需要事先给出一个pk。a乘了两次，是因为pk这里pk是平方误差。

卡尔曼增益
卡尔曼增益表示应该更倾向于观测值还是预测值。K = pk / (pk + r), 当pk小 而r大时，表示观测误差大，倾向于预测值，反之亦然。

最佳估计
那么就可以选最佳估计了， x = 当前估计 + K * (观测值 - a1*估计值) ,这里是和观测方程有联系，最佳估计就在观测值和预测值之间，通过预测误差的处理得到增益K，计算最佳估计。

误差更新
卡尔曼的精髓，将上个状态的误差计算到下个状态中，所以这里要更新pk
pk = pk*（1-K）。