update method

void update(
  1. Matrix? z, {
  2. List? args,
})

Update function of the Kalman Filter

Implementation

void update(Matrix? z, {List? args}) {
  if (z == null) {
    return;
  }

  List<List<double>> sigmasH = [];
  for (var element in _sigmasF) {
    sigmasH.add(_hx(Matrix.row(element.toList()), args)[0].toList());
  }

  _sigmasH = atLeast2D(Matrix.fromList(sigmasH));
  List<Matrix> ut =
      unscentedTransform(_sigmasH!, _weightsMean, _weightsCov, noiseCov: r);
  Matrix zp = ut[0];
  _s = ut[1];
  _sInverted = toInverse(_s);
  Matrix pxz = _crossVariance(x, zp, _sigmasF, _sigmasH!);
  _k = pxz * _sInverted;
  _y = z - zp;
  x = x + (_k * _y.toVector()).transpose();
  p = p - (_k * (_s * _k.transpose()));
}