This document gives the skeleton of the derivation of a matrix (higher order) Kalman filter. The math follows in the same was as the scalar case, but uses matrix variables. Rigorous derivations are given in the references.
The matrix problem proceeds just as the scalar problem, but all of the variables are now matrices. The system has p inputs, n state variables and m outputs.
variable Scalar Matrix Matrix size state xj xj n by 1 input uj uj p by 1 output zj zj m by 1 state gain a A n by n input gain b B n by p output gain h Hj m by n process noise wj wj n by 1 process noise covariance Q Q n by n measurement noise vj vj m by 1 measurement noise covariance R R m by m a priori covariance n by n a posteriori covariance n by n Kalman Filter Gain kj Kj n by m
The solution proceeds as did the scalar case. Only major results will be presented here, the intermediate results are not discussed, but can be determined by analogy with the derivation of the scalar kalman filter.
A discrete time system with process noise w and measurement noise v is defined by:
The corresponding block diagram is shown below
The block diagram for a Kalman filter is given by:
The predictor equation is given by
The corrector equation is given by
The a priori and a posteriori covariances are given by
where the superscript T denotes the matrix transpose.
To find the best value for the filter gain, Kj, differentiate the a posteriori covariance and set it to zero:
The Kalman filter gain is obtained after much algebra and is given by
The recursive form of the a priori covariance is given by:
The recursive calculation of the a posteriori covariance is given by:
Equations 2 through 6 give the Kalman filter algorithm. The other equations are only necessary for the derivation of the gain and covariances.