Package com.irurueta.numerical.signal.processing

Class KalmanFilter

java.lang.Object
com.irurueta.numerical.signal.processing.KalmanFilter
All Implemented Interfaces:
Serializable
public class KalmanFilter extends Object implements Serializable
Implementation of a Kalman filter. This class contains the state of a Kalman kilter. The state of a Kalman filter is updated by predict and correct functions.

The source code, notation and formulae below are borrowed from the JKalman tutorial [Welch95]: 

 
 x<sub>k</sub>=A*x<sub>k-1</sub>+B*u<sub>k</sub>+w<sub>k</sub>
 z<sub>k</sub>=Hx<sub>k</sub>+v<sub>k</sub>,

where: 

 x<sub>k</sub> (x<sub>k-1</sub>) - state of the system at the moment k (k-1)
 z<sub>k</sub> - measurement of the system state at the moment k
 u<sub>k</sub> - external control applied at the moment k
 w<sub>k</sub> and v<sub>k</sub> are normally-distributed process and
 measurement noise, respectively:
 p(w) ~ N(0,Q)
 p(v) ~ N(0,R),
 that is,
 Q - process noise covariance matrix, constant or variable,
 R - measurement noise covariance matrix, constant or variable

In case of standard Kalman filter, all the matrices: A, B, H, Q and R are initialized once after Kalman structure is allocated via constructor. However, the same structure and the same functions may be used to simulate extended Kalman filter by linearizing extended Kalman filter equation in the current system state neighborhood, in this case A, B, H (and, probably, Q and R) should be updated on every step.

See Also:

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private com.irurueta.algebra.Matrix
    controlMatrix
    Control matrix (B) (it is not used if there is no control).
    private final int
    cp
    Number of control vector dimensions (control parameters).
    static final double
    DEFAULT_MEASUREMENT_NOISE_VARIANCE
    Independent measurement noise variance assumed when no measurement noise covariance matrix is provided.
    static final double
    DEFAULT_PROCESS_NOISE_VARIANCE
    Independent process noise variance assumed when no process noise covariance matrix is provided.
    private final int
    dp
    Number of state vector dimensions (dynamic parameters).
    private com.irurueta.algebra.Matrix
    errorCovPost
    Posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
    private com.irurueta.algebra.Matrix
    errorCovPre
    Priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)
    private com.irurueta.algebra.Matrix
    gain
    Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    private com.irurueta.algebra.Matrix
    measurementMatrix
    Measurement matrix (H).
    private com.irurueta.algebra.Matrix
    measurementNoiseCov
    Measurement noise covariance matrix (R).
    private int
    mp
    Number of measurement vector dimensions (measure parameters).
    private com.irurueta.algebra.Matrix
    processNoiseCov
    Process noise covariance matrix (Q).
    private com.irurueta.algebra.Matrix
    statePost
    Corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    private com.irurueta.algebra.Matrix
    statePre
    Predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    private final com.irurueta.algebra.Matrix
    temp1
    Temporary matrix 1.
    private com.irurueta.algebra.Matrix
    temp2
    Temporary matrix 2.
    private com.irurueta.algebra.Matrix
    temp3
    Temporary matrix 3.
    private com.irurueta.algebra.Matrix
    temp4
    Temporary matrix 4.
    private com.irurueta.algebra.Matrix
    temp5
    Temporary matrix 5.
    private com.irurueta.algebra.Matrix
    temp6
    Temporary matrix 6.
    private final com.irurueta.algebra.Matrix
    temp7
    Temporary matrix 7.
    private com.irurueta.algebra.Matrix
    temp8
    Temporary matrix 8.
    private com.irurueta.algebra.Matrix
    transitionMatrix
    State transition matrix (A).

  • Constructor Summary

    Constructors
    Constructor
    Description
    KalmanFilter(int dynamParams, int measureParams)
    Constructor in case of no control parameters.
    KalmanFilter(int dynamParams, int measureParams, int controlParams)
    Allocates a Kalman filter and all its matrices and initializes them.

  • Method Summary

    Modifier and Type
    Method
    Description
    com.irurueta.algebra.Matrix
    correct(com.irurueta.algebra.Matrix measurement)
    Adjusts model state.
    com.irurueta.algebra.Matrix
    getControlMatrix()
    Obtains the control matrix (B) (it is not used if there is no control).
    int
    getControlParameters()
    Obtains the number of control vector dimensions (control parameters).
    int
    getDynamicParameters()
    Obtains the number of state vector dimensions (dynamic parameters).
    com.irurueta.algebra.Matrix
    getErrorCovPost()
    Obtains the posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k).
    com.irurueta.algebra.Matrix
    getErrorCovPre()
    Obtains the priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q).
    com.irurueta.algebra.Matrix
    getGain()
    Obtains the Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R).
    com.irurueta.algebra.Matrix
    getMeasurementMatrix()
    Obtains measurement matrix (H).
    com.irurueta.algebra.Matrix
    getMeasurementNoiseCov()
    Obtains the measurement noise covariance matrix (R).
    int
    getMeasureParameters()
    Obtains the number of measurement vector dimensions (measure parameters).
    com.irurueta.algebra.Matrix
    getProcessNoiseCov()
    Obtains the process noise covariance matrix (Q).
    com.irurueta.algebra.Matrix
    getStatePost()
    Obtains corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)).
    com.irurueta.algebra.Matrix
    getStatePre()
    Obtains predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k).
    com.irurueta.algebra.Matrix
    getTransitionMatrix()
    Obtains the state transition matrix (A).
    com.irurueta.algebra.Matrix
    predict()
    Estimates subsequent model state without control parameters.
    com.irurueta.algebra.Matrix
    predict(com.irurueta.algebra.Matrix control)
    Estimates subsequent model state.
    void
    setControlMatrix(com.irurueta.algebra.Matrix controlMatrix)
    Sets the control matrix (B) (it is not used if there is no control).
    void
    setErrorCovPost(com.irurueta.algebra.Matrix errorCovPost)
    Sets the posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k).
    void
    setErrorCovPre(com.irurueta.algebra.Matrix errorCovPre)
    Sets the priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q).
    void
    setGain(com.irurueta.algebra.Matrix gain)
    Sets the Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R).
    void
    setMeasurementMatrix(com.irurueta.algebra.Matrix measurementMatrix)
    Sets measurement matrix (H).
    void
    setMeasurementNoiseCov(com.irurueta.algebra.Matrix measurementNoiseCov)
    Sets the measurement noise covariance matrix (R).
    void
    setMeasureParameters(int measureParameters)
    Sets the number of measurement vector dimensions (measure parameters).
    void
    setProcessNoiseCov(com.irurueta.algebra.Matrix processNoiseCov)
    Sets the process noise covariance matrix (Q).
    void
    setStatePost(com.irurueta.algebra.Matrix statePost)
    Sets corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)).
    void
    setStatePre(com.irurueta.algebra.Matrix statePre)
    Sets predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k).
    void
    setTransitionMatrix(com.irurueta.algebra.Matrix transitionMatrix)
    Sets the state transition matrix (A).

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • DEFAULT_PROCESS_NOISE_VARIANCE

      public static final double DEFAULT_PROCESS_NOISE_VARIANCE
      Independent process noise variance assumed when no process noise covariance matrix is provided. The lower the process variance the smoother the estimated state will typically be.
      See Also:

    • DEFAULT_MEASUREMENT_NOISE_VARIANCE

      public static final double DEFAULT_MEASUREMENT_NOISE_VARIANCE
      Independent measurement noise variance assumed when no measurement noise covariance matrix is provided.
      See Also:

    • mp

      private int mp
      Number of measurement vector dimensions (measure parameters).

    • dp

      private final int dp
      Number of state vector dimensions (dynamic parameters).

    • cp

      private final int cp
      Number of control vector dimensions (control parameters).

    • statePre

      private com.irurueta.algebra.Matrix statePre
      Predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)

    • statePost

      private com.irurueta.algebra.Matrix statePost
      Corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))

    • transitionMatrix

      private com.irurueta.algebra.Matrix transitionMatrix
      State transition matrix (A).

    • controlMatrix

      private com.irurueta.algebra.Matrix controlMatrix
      Control matrix (B) (it is not used if there is no control).

    • measurementMatrix

      private com.irurueta.algebra.Matrix measurementMatrix
      Measurement matrix (H).

    • processNoiseCov

      private com.irurueta.algebra.Matrix processNoiseCov
      Process noise covariance matrix (Q).

    • measurementNoiseCov

      private com.irurueta.algebra.Matrix measurementNoiseCov
      Measurement noise covariance matrix (R).

    • errorCovPre

      private com.irurueta.algebra.Matrix errorCovPre
      Priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)

    • gain

      private com.irurueta.algebra.Matrix gain
      Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)

    • errorCovPost

      private com.irurueta.algebra.Matrix errorCovPost
      Posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    • temp1

      private final com.irurueta.algebra.Matrix temp1
      Temporary matrix 1.

    • temp2

      private com.irurueta.algebra.Matrix temp2
      Temporary matrix 2.

    • temp3

      private com.irurueta.algebra.Matrix temp3
      Temporary matrix 3.

    • temp4

      private com.irurueta.algebra.Matrix temp4
      Temporary matrix 4.

    • temp5

      private com.irurueta.algebra.Matrix temp5
      Temporary matrix 5.

    • temp6

      private com.irurueta.algebra.Matrix temp6
      Temporary matrix 6.

    • temp7

      private final com.irurueta.algebra.Matrix temp7
      Temporary matrix 7.

    • temp8

      private com.irurueta.algebra.Matrix temp8
      Temporary matrix 8.

  • Constructor Details

    • KalmanFilter

      public KalmanFilter(int dynamParams, int measureParams, int controlParams) throws SignalProcessingException
      Allocates a Kalman filter and all its matrices and initializes them.
      Parameters:
      dynamParams - number of dynamic parameters (state vector dimensions).
      measureParams - number of measurement parameters (measurement vector dimensions).
      controlParams - number of control parameters (control vector. dimensions). If zero, no control parameters are used. If less than zero, it is assumed that this is equal to the number of dynamic parameters.
      Throws:
      IllegalArgumentException - if either the number of dynamic or measurement parameters is zero or negative.
      SignalProcessingException - if something else fails.

    • KalmanFilter

      public KalmanFilter(int dynamParams, int measureParams) throws SignalProcessingException
      Constructor in case of no control parameters.
      Parameters:
      dynamParams - number of dynamic parameters (state vector dimensions).
      measureParams - number of measurement parameters (measurement vector dimensions).
      Throws:
      IllegalArgumentException - if either the number of dynamic or measurement parameters is zero or negative.
      SignalProcessingException - if something else fails.

  • Method Details

    • predict

      public com.irurueta.algebra.Matrix predict() throws SignalProcessingException
      Estimates subsequent model state without control parameters.
      Returns:
      estimated state.
      Throws:
      SignalProcessingException - if something fails.
      See Also:

    • predict

      public com.irurueta.algebra.Matrix predict(com.irurueta.algebra.Matrix control) throws SignalProcessingException
      Estimates subsequent model state. The function estimates the subsequent stochastic model state by its current state and stores it at statePre: 
       
 x'<sub>k</sub>=A*x<sub>k</sub>+B*u<sub>k</sub>
 P'<sub>k</sub>=A*P<sub>k-1</sub>*A<sup>T</sup> + Q,
 where
 x'<sub>k</sub> is predicted state (statePre),
 x<sub>k-1</sub> is corrected state on the previous step (statePost)
     (should be initialized somehow in the beginning, zero vector by
 default),
 u<sub>k</sub> is external control (<code>control</code> parameter),
 P'<sub>k</sub> is prior error covariance matrix (error_cov_pre)
 P<sub>k-1</sub> is posteriori error covariance matrix on the previous
 step (error_cov_post)
     (should be initialized somehow in the beginning, identity matrix by
 default),
      Parameters:
      control - control vector (uk), should be null if there is no external control (controlParams=0). If provided and filter uses control parameters, it must be a 1 column matrix having cp rows (where cp = number of control parameters), otherwise a SignalProcessingException will be raised.
      Returns:
      estimated state as a 1 column matrix having dp rows (where dp = number of dynamic parameters).
      Throws:
      SignalProcessingException - if something fails.

    • correct

      public com.irurueta.algebra.Matrix correct(com.irurueta.algebra.Matrix measurement) throws SignalProcessingException
      Adjusts model state. This method adjusts stochastic model state on the basis of the given measurement of the model state: 
       
 K<sub>k</sub>=P'<sub>k</sub>*H<sup>T</sup>*(H*P'<sub>k</sub>*H<sup>T</sup>+R)<sup>-1</sup>
 x<sub>k</sub>=x'<sub>k</sub>+K<sub>k</sub>*(z<sub>k</sub>-H*x'<sub>k</sub>)
 P<sub>k</sub>=(I-K<sub>k</sub>*H)*P'<sub>k</sub>
 where
 z<sub>k</sub> - given measurement (<code>measurement</code> parameter)
 K<sub>k</sub> - Kalman "gain" matrix.

      The function stores adjusted state at statePost and returns it on output.

      Parameters:
      measurement - matrix containing the measurement vector. Matrix must have 1 column and mp rows (mp = measurement parameters).
      Returns:
      adjusted model state.
      Throws:
      SignalProcessingException - if something fails.

    • getMeasureParameters

      public int getMeasureParameters()
      Obtains the number of measurement vector dimensions (measure parameters).
      Returns:
      number of measurement vector dimensions (measure parameters)

    • setMeasureParameters

      public void setMeasureParameters(int measureParameters) throws SignalProcessingException
      Sets the number of measurement vector dimensions (measure parameters).
      Parameters:
      measureParameters - number of measurement vector dimensions (measure parameters). NOTE: when resetting number of measure parameters, the measurement noise covariance matrix and the measurement matrix get reset to their default values having the required new size. Please, make sure those matrices are reset to their proper values after calling this method.
      Throws:
      IllegalArgumentException - if provided value is zero or negative.
      SignalProcessingException - if something else fails

    • getDynamicParameters

      public int getDynamicParameters()
      Obtains the number of state vector dimensions (dynamic parameters).
      Returns:
      number of state vector dimensions (dynamic parameters)

    • getControlParameters

      public int getControlParameters()
      Obtains the number of control vector dimensions (control parameters).
      Returns:
      number of control vector dimensions (control parameters)

    • getStatePre

      public com.irurueta.algebra.Matrix getStatePre()
      Obtains predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k). It is a column matrix having 1 column and dp rows, where dp is the number of dynamic parameters
      Returns:
      predicted state

    • setStatePre

      public void setStatePre(com.irurueta.algebra.Matrix statePre)
      Sets predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k). Provided matrix must have 1 column and dp rows, where dp is the number of dynamic parameters set for this Kalman filter instance. This setter method can be used for initial setup purposes.
      Parameters:
      statePre - new predicted state.
      Throws:
      IllegalArgumentException - if provided matrix does not have 1 column and dp rows

    • getStatePost

      public com.irurueta.algebra.Matrix getStatePost()
      Obtains corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)). It is a column matrix having 1 column and dp rows, where dp is the number of dynamic parameters
      Returns:
      corrected state

    • setStatePost

      public void setStatePost(com.irurueta.algebra.Matrix statePost)
      Sets corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)). Provided matrix must have 1 column and dp rows, where dp is the number of dynamic parameters set for this Kalman filter instance. This setter method can be used for initial setup purposes.
      Parameters:
      statePost - new corrected state
      Throws:
      IllegalArgumentException - if provided matrix does not have 1 column and dp rows

    • getTransitionMatrix

      public com.irurueta.algebra.Matrix getTransitionMatrix()
      Obtains the state transition matrix (A). It is a square matrix having dp rows and columns, where dp is equal to the number of dynamic parameters. This matrix defines how the system transitions to a new state for a given previous state. It is used for prediction purposes
      Returns:
      state transition matrix

    • setTransitionMatrix

      public void setTransitionMatrix(com.irurueta.algebra.Matrix transitionMatrix)
      Sets the state transition matrix (A). It must be a square matrix having dp rows and columns, where dp is equal to the number of dynamic parameters set for this instance. This matrix defines how the system transitions to a new state for a given previous state. It is used for prediction purposes. This setter method can be used for initial setup purposes.
      Parameters:
      transitionMatrix - new state transition matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and columns

    • getControlMatrix

      public com.irurueta.algebra.Matrix getControlMatrix()
      Obtains the control matrix (B) (it is not used if there is no control). It's a matrix having dp rows and cp columns, where dp is the number of dynamic parameters and cp is the number of control parameters.
      Returns:
      control matrix

    • setControlMatrix

      public void setControlMatrix(com.irurueta.algebra.Matrix controlMatrix)
      Sets the control matrix (B) (it is not used if there is no control). Provided matrix must have dp rows and cp columns, where dp is the number of dynamic parameters and cp is the number of control parameters set for this Kalman filter instance. This setter method can be used for initial setup purposes.
      Parameters:
      controlMatrix - new control matrix to be set, or null if no control parameters are set
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and cp columns

    • getMeasurementMatrix

      public com.irurueta.algebra.Matrix getMeasurementMatrix()
      Obtains measurement matrix (H). It's a matrix having mp rows and dp columns, where mp is the number of measurement parameters and dp is the number of dynamic parameters of the system state. This matrix relates obtained measures to the actual system state when a given model is known in advance. If no model is known and measures directly indicate the system state, then this matrix must be the identity.
      Returns:
      measurement matrix

    • setMeasurementMatrix

      public void setMeasurementMatrix(com.irurueta.algebra.Matrix measurementMatrix)
      Sets measurement matrix (H). Provided matrix must have mp rows and dp columns, where mp is the number of measurement parameters and dp is the number of dynamic parameters of the system state. This matrix relates obtained measures to the actual system state when a given model is known in advance. If no model is known and measures directly indicate the system state, then this matrix must be the identity. This setter method can be used for initial setup purposes.
      Parameters:
      measurementMatrix - measurement matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have mp rows and dp columns.

    • getProcessNoiseCov

      public com.irurueta.algebra.Matrix getProcessNoiseCov()
      Obtains the process noise covariance matrix (Q). This is a covariance matrix indicating the correlations of the amount of error in the system state. It is a square symmetric matrix having dp rows and columns, where dp is the number of dynamic parameters containing the system state.
      Returns:
      the process noise covariance matrix

    • setProcessNoiseCov

      public void setProcessNoiseCov(com.irurueta.algebra.Matrix processNoiseCov)
      Sets the process noise covariance matrix (Q). This is a covariance matrix indicating the correlations of the amount of error in the system state. It must be provided a square symmetric matrix having dp rows and columns, where dp is the number of dynamic parameters containing the system state for this instance of a Kalman filter. This setter method can be used for initial setup purposes, however typically the process noise is difficult to determine. This matrix is generally constructed intuitively so that un-modelled dynamics and parameter uncertainties are modeled as process noise generally. If the process noise is unknown, just leave the default value or provide a diagonal matrix with the desired level of variance Q, where a low Q variance indicates confidence that any unknown noise terms and/or modelling errors are small to negligible, and higher Q allows the tracker to follow the state despite unknown noise and/or model errors.
      Parameters:
      processNoiseCov - process noise covariance matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and columns, or it is not symmetric

    • getMeasurementNoiseCov

      public com.irurueta.algebra.Matrix getMeasurementNoiseCov()
      Obtains the measurement noise covariance matrix (R). This is a covariance matrix indicating the correlations of the amount of error in the measures taken from the system. It is a square symmetric matrix having mp rows and columns, where mp is the number of measurement parameters. Typically, this matrix can be easily obtained by processing the measurements while the output of the system is held constant. In this case, only noise remains in the data after its mean is removed. The covariance can be calculated easily from the remaining portion of the data.
      Returns:
      the measurement noise covariance matrix

    • setMeasurementNoiseCov

      public void setMeasurementNoiseCov(com.irurueta.algebra.Matrix measurementNoiseCov)
      Sets the measurement noise covariance matrix (R). This is a covariance matrix indicating the correlations of the amount of error in the measures taken from the system. Provided matrix must be a square symmetric matrix having mp rows and columns, where mp is the number of measurement parameters. Typically, this matrix can be easily obtained by processing the measurements while the output of the system is held constant. In this case, only noise remains in the data after its mean is removed. The covariance can be calculated easily from the remaining portion of the data. This setter method can be used for initial setup purposes.
      Parameters:
      measurementNoiseCov - new measurement noise covariance matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have mp rows and columns, or it is not symmetric

    • getErrorCovPre

      public com.irurueta.algebra.Matrix getErrorCovPre()
      Obtains the priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q). It is a square symmetric matrix having dp rows and columns, where dp is the number of dynamic parameters of the system state
      Returns:
      the priori error estimate covariance matrix

    • setErrorCovPre

      public void setErrorCovPre(com.irurueta.algebra.Matrix errorCovPre)
      Sets the priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q). Provided matrix must be square and symmetric having dp rows and columns, where dp is the number of the dynamic parameters of the system state set for this Kalman filter instance. This setter method can be used for initial setup purposes, however this value will rarely need to be set, and instead the getter method will be used to obtain the error of the predicted system state once the filter converges
      Parameters:
      errorCovPre - new priori error estimate covariance matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and columns, or it is not symmetric

    • getGain

      public com.irurueta.algebra.Matrix getGain()
      Obtains the Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R). This matrix is used to correct the predicted state, if the gain values are small then the filter is accurately tracking the system state and the prediction error remains small too. The gain matrix has dp rows and mp columns, where dp is the number of dynamic parameters and mp is the number of measure parameters.
      Returns:
      the Kalman gain matrix

    • setGain

      public void setGain(com.irurueta.algebra.Matrix gain)
      Sets the Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R). This matrix is used to correct the predicted state, if the gain values are small then the filter is accurately tracking the system state and the prediction error remains small too. The gain matrix must have dp rows and mp columns, where dp is the number of dynamic parameters and mp is the number of measure parameters set for this Kalman filter instance. This setter method can be used for initial setup purposes, however this matrix rarely needs to be set, and instead it is better to let the filter converge to the actual system state.
      Parameters:
      gain - new gain matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and mp columns

    • getErrorCovPost

      public com.irurueta.algebra.Matrix getErrorCovPost()
      Obtains the posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k). It is a square symmetric matrix having dp rows and columns, where dp is the number of dynamic parameters of the system state
      Returns:
      the priori error estimate covariance matrix

    • setErrorCovPost

      public void setErrorCovPost(com.irurueta.algebra.Matrix errorCovPost)
      Sets the posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k). Provided matrix must be square and symmetric having dp rows and columns, where dp is the number of the dynamic parameters of the system state set for this Kalman filter instance. This setter method can be used for initial setup purposes, however this value will rarely need to be set, and instead the getter method will be used to obtain the error of the posteriori system state once the filter converges
      Parameters:
      errorCovPost - new posteriori error estimate covariance matrix
      Throws:
      IllegalArgumentException - if provided matrix does not have dp rows and columns, or it is not symmetric