unscentedTransform method

List<Matrix> unscentedTransform(
  1. Matrix sigmas,
  2. Matrix weightsM,
  3. Matrix weightsC, {
  4. Matrix? noiseCov,
})

Implementation

List<Matrix> unscentedTransform(
    Matrix sigmas, Matrix weightsM, Matrix weightsC,
    {Matrix? noiseCov}) {
  Matrix newP = filledWithZeros(sigmas.columnsNum);
  Matrix newX = weightsM * sigmas;
  Matrix newY = sigmas
      .mapRows((row) => row - newX.toVector()); // sigmas - x.toVector();
  newP = newY.transpose() * (Matrix.diagonal(weightsC[0].toList()) * newY);

  if (noiseCov != null) {
    if (noiseCov.length == 1) {
      newP = newP + noiseCov[0][0];
    } else {
      newP = newP + noiseCov;
    }
  }

  return [newX, newP];
}