Update( Matrix z, Matrix H, Matrix R ) { KalmanFilter.CheckUpdateParameters(z, H, R, this); // Fiddle with the matrices Matrix HT = Matrix.Transpose(H); Matrix RI = R.Inverse(); // Perform the update this.y = this.y + (HT * RI * z); this.J = this.J + (HT * RI * H); }
Update( Matrix z, Matrix H, Matrix R ) { KalmanFilter.CheckUpdateParameters(z, H, R, this); // We need to use transpose of H a couple of times. Matrix Ht = Matrix.Transpose(H); Matrix I = Matrix.Identity(x.RowCount, x.RowCount); Matrix S = (H * P * Ht) + R; // Measurement covariance Matrix K = P * Ht * S.Inverse(); // Kalman Gain this.P = (I - (K * H)) * P; // Covariance update this.x = this.x + (K * (z - (H * this.x))); // State update }