.Modelica_LinearSystems2.WorkInProgress.DiscreteStateSpace.Internal.kfStepMatrices

Information

One step, i.e. prediction and update of a kalman filter iteration for discrete systems

Interface

function kfStepMatrices
  extends Modelica.Icons.Function;
  import Modelica;
  import Modelica_LinearSystems2;
  import Modelica_LinearSystems2.Math;
  import Modelica_LinearSystems2.Math.Matrices.LAPACK;
  import Modelica_LinearSystems2.WorkInProgress.DiscreteStateSpace;
  input Real A[:, size(A, 1)] "Transition matrix of the discrete system";
  input Real B[size(A, 1), :] "Input matrix of the discrete system";
  input Real C[:, size(A, 1)] "Output matrix of the discrete system";
  input Real P[size(A, 1), size(A, 1)] "State covariance matrix of the previous instant";
  input Real Q[size(B, 2), size(B, 2)] "Input or process noise covariance matrix of the previous instant";
  input Real R[size(C, 1), size(C, 1)] "Output or measurement noise covariance matrix of the previous instant";
  output Real K[size(A, 1), size(C, 1)] = P*transpose(C) "Kalman filter gain matrix";
  output Real P_new[size(A, 1), size(A, 1)] "Updated state covariance matrix";
  output Real UMutri[size(C, 1), size(C, 1)] "Square root (left Cholesky factor) of the covariance matrix M";
  output Real M[size(C, 1), size(C, 1)] = DiscreteStateSpace.Internal.symMatMul(C, P, R, true) "Upper triangle of measurement prediction covariance C*P*C' + R";
  output Real PCT[size(A, 1), size(C, 1)] = P*transpose(C) "Matrix P*C'";
end kfStepMatrices;