卡尔曼滤波算法的应用JavaScript实现案例

从单纯使用卡尔曼滤波算法的角度出发，那么我们先刨去卡尔曼滤波算法的内核公式，得到主要的应用框架公式就是：
最优估计值 = 观测值 + 卡尔曼系数 * （估计值-观测值）
这就是卡尔曼算法的应用。
其中：
观测值是传感器输入的数据，比如卫星定位获取到的坐标数据（x,y）
估计值是通过数学公式计算估计的数据，比如 沿直线运动，计算一定时间后应该在的位置：x1 = x0 + v * t

​​​​​​​看起来很简单是吧。

但是卡尔曼系数该如何获取呢？
卡尔曼系数的获得便是整个卡尔曼算法的难点和核心：
预测状态值公式： $x[t] = F * x[t-1] + B * u[t-1]  其中F为状态转换矩阵，B为控制矩阵
预测协方差矩阵公式：$P[t] = F * P[t-1] * F(T) + Q 其中Q为预测模型本身存在的噪声的协方差矩阵
观测状态值公式：z[t] = H * x[t] + v 其中 H为最优估计值到观测值间的关系矩阵，v为观测值噪声 v的协方差矩阵为R
卡尔曼系数：K[t] = $P[t] * H(T) / ( H * $P[t] * H(T) + R ) // 其中R为观测观测值噪声协方差
最优估计值公式：x[t] = $x[t] + K[t] * ( z[t] - H * $x[t] )
更新状态噪声协方差矩阵：P[t] =  ( I - K[t]H) * $P[t]

对于以上的公式可以不需要理解透，但是要能够知道代入什么参数，之后才能进行代码的编写。这里的公式理解和使用可以参考后面的的视频链接学习理解

案例代码内仅对单一方向上的分量值进行卡尔曼过滤，多个分量的情况下请多次使用（如：在预估平面运动的情况下，需要分别进行在x轴和y轴方向上的滤波计算，控制量也需要映射到x和y轴后再带入计算）。

每次计算都会依赖之前的数据结果，所以需要利用闭包设置缓存数据。

 参考视频：【不要再看那些过时的卡尔曼滤波老教程了】2022巨献，卡尔曼滤波-目标追踪从理论到实战最新版全套教程！建议收藏_哔哩哔哩_bilibili从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili

滤波算法：

/**
 * 卡尔曼滤波算法
 * @param {观测值} z 
 * @param {控制量} a
 * @author Su Jiantao
 */
export function kalmanFilter(defaultd) {
  // 预测状态值公式： $x[t] = F * x[t-1] + B * u[t-1]  其中F为状态转换矩阵，B为控制矩阵
  // 预测协方差矩阵公式：$P[t] = F * P[t-1] * F(T) + Q 其中Q为预测模型本身存在的噪声的协方差矩阵
  // 观测状态值公式：z[t] = H * x[t] + v 其中 H为最优估计值到观测值间的关系矩阵，v为观测值噪声 v的协方差矩阵为R
  // 卡尔曼系数：K[t] = $P[t] * H(T) / ( H * $P[t] * H(T) + R ) // 其中R为观测观测值噪声协方差
  // 最优估计值公式：x[t] = $x[t] + K[t] * ( z[t] - H * $x[t] )
  // 更新状态噪声协方差矩阵：P[t] =  ( I - K[t]H) * $P[t]

  let d = defaultd, v = 0
  const P = [
    [1, 0],
    [0, 1]
  ]

  // const F = [
  //   [1, dt],
  //   [0, 1]
  // ]
  const Q = [
    [0.08, 0], // 位置预测噪声方差
    [0, 1] // 速度预测噪声方差
  ]
  // const H = [
  //   [1, 0]
  // ]
  const R = 1 // 观测值的噪声是一维的(只接受一个传感器参数)所以直接取观测值噪声方差
  // Q和R需要手动进行超参数调整，调整到合适的数值
  return (z, a, dt) => {
      // 预测
      const $d = d + v * dt + 0.5 * a * dt * dt // a已经包含误差，所以不用在加预测噪声
      const $v = v + dt * a
      const $P00 = P[0][0] + Q[0][0] + (P[0][1] + P[1][0]) * dt  + dt * dt * P[1][1]  // dt * dt * P[1][1]通常该值太小可忽略
      const $P01 = P[0][1] + P[1][1] * dt
      const $P10 = P[1][0] + P[1][1] * dt
      const $P11 = P[1][1] + Q[1][1]
      const K0 = $P00 / ($P00 + R)
      const K1 = $P10 / ($P00 + R)


      // 最优估计值
      v = $v + K1 * ((z - d) / dt - $v)
      d = $d + K0 * (z - $d)
      P[0][0] = $P00 - K0 * $P00
      P[0][1] = $P01 - K0 * $P01
      P[1][0] = $P10 - K1 * $P00
      P[1][1] = $P11 - K1 * $P01
      return d
  }
}

每次获取到观测值就执行以下代码：
(dt 是上次执行与本次执行的时间间隔，第一次执行dt是0)

if (getKalmanValueX == null) {
    getKalmanValueX = kalmanFilter(z.x)
  }
if (getKalmanValueY == null) {
   getKalmanValueY = kalmanFilter(z.y)
 }
  const x = getKalmanValueX(z.x, a_x, dt)
  const y = getKalmanValueY(z.y, a_y, dt)