Matrix.Single transition; // State transition matrix (A). #endregion Fields #region Constructors public Kalman( Matrix.Single transition, Matrix.Single measurement, Matrix.Single measurementNoiseCovariance, Matrix.Single processNoiseCovariance, Matrix.Single initialState, Matrix.Single initialErrorCovariance, Matrix.Single control) { int dynamicParameters = transition.Order; int measureParameters = measurementNoiseCovariance.Order; this.transition = transition; this.processNoiseCovariance = processNoiseCovariance; this.measurement = measurement; this.measurementNoiseCovariance = measurementNoiseCovariance; this.statePostCorrected = initialState; this.errorCovariancePosteriori = initialErrorCovariance; this.statePredicted = new Kean.Math.Matrix.Single(1, dynamicParameters); this.errorCovariancePriori = new Kean.Math.Matrix.Single(dynamicParameters); this.gain = new Kean.Math.Matrix.Single(measureParameters, dynamicParameters); this.control = control; }
public Matrix.Single Correct(Matrix.Single measurement) { Matrix.Single result = this.statePostCorrected; if (measurement.NotNull()) { // a = H*P'(k) Matrix.Single b = this.measurement * this.errorCovariancePriori; // b = temp2*Ht + R Matrix.Single a = b * this.measurement.Transpose() + this.measurementNoiseCovariance; // c = inv(a) * b Matrix.Single c = a.Solve(b); // K(k) = xt this.gain = c.Transpose(); // x(k) = x'(k) + K(k)*(z(k) - H*x'(k)) result = this.statePostCorrected = this.statePredicted + this.gain * (measurement - this.measurement * this.statePredicted); // P(k) = P'(k) - K(k)*temp2 this.errorCovariancePosteriori = this.errorCovariancePriori - this.gain * b; } return result; }
public Matrix.Single Predict(Matrix.Single control) { Matrix.Single result; // Update state // x'(k) = A*x(k) this.statePredicted = this.transition * this.statePostCorrected; // x'(k) = x'(k) + B*u(k) if (this.control.NotNull() && control.NotNull()) this.statePredicted += this.control * control; // update error covariance // P'(k) = A*P(k)*At + Q this.errorCovariancePriori = (this.transition * this.errorCovariancePosteriori) * this.transition.Transpose() + this.processNoiseCovariance; result = this.statePredicted; return result; }