コード例 #1
0
ファイル: Kalman.cs プロジェクト: imintsystems/Kean
        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;
        }
コード例 #2
0
ファイル: Kalman.cs プロジェクト: imintsystems/Kean
 public Kalman(int dynamicParameters, int measureParameters, int controlParameters)
 {
     this.statePredicted = new Kean.Math.Matrix.Single(1, dynamicParameters);
     this.statePostCorrected = new Kean.Math.Matrix.Single(1, dynamicParameters);
     this.transition = Matrix.Single.Identity(dynamicParameters);
     this.processNoiseCovariance = Matrix.Single.Identity(dynamicParameters);
     this.measurement = new Kean.Math.Matrix.Single(dynamicParameters, measureParameters);
     this.measurementNoiseCovariance = Matrix.Single.Identity(measureParameters);
     this.errorCovariancePriori = new Kean.Math.Matrix.Single(dynamicParameters);
     this.errorCovariancePosteriori = new Kean.Math.Matrix.Single(dynamicParameters);
     this.gain = new Kean.Math.Matrix.Single(measureParameters, dynamicParameters);
     if (controlParameters > 0)
         this.control = new Kean.Math.Matrix.Single(controlParameters, dynamicParameters);
 }
コード例 #3
0
ファイル: Kalman.cs プロジェクト: imintsystems/Kean
 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;
 }
コード例 #4
0
ファイル: Kalman.cs プロジェクト: imintsystems/Kean
 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;
 }
コード例 #5
0
ファイル: Single.cs プロジェクト: imintsystems/Kean
        Element[] Natural(Tuple<float, float>[] measures)
        {
            // Compute natural piecewise cubic splines.
            Element[] result = new Element[measures.Length - 1];
            Matrix.Single left = new Matrix.Single(measures.Length, measures.Length);
            Matrix.Single right = new Matrix.Single(1, measures.Length);
            left[0, 0] = 2;
            left[1, 0] = 1;
            left[measures.Length - 1, measures.Length - 1] = 2;
            left[measures.Length - 2, measures.Length - 1] = 1;
            for (int y = 1; y < measures.Length - 1; y++)
            {
                int x = y - 1;
                left[x, y] = 1;
                left[x + 1, y] = 4;
                left[x + 2, y] = 1;
            }
            right[0, 0] = 3 * (measures[1].Item2 - measures[0].Item2);
            right[0, measures.Length - 1] = 3 * (measures[measures.Length - 1].Item2 - measures[measures.Length - 2].Item2);
            for (int y = 1; y < measures.Length - 1; y++)
                right[0, y] = 3 * (measures[y + 1].Item2 - measures[y - 1].Item2);
            Matrix.Single solution = left.Solve(right);

            for (int x = 0; x < measures.Length - 1; x++)
            {
                float a = measures[x].Item2;
                float b = solution[0, x];
                float c = 3 * (measures[x + 1].Item2 - measures[x].Item2) - 2 * solution[0, x] - solution[0, x + 1];
                float d = 2 * (measures[x].Item2 - measures[x + 1].Item2) + solution[0, x] + solution[0, x + 1];
                result[x] = new Element(a, b, c, d, measures[x].Item1, measures[x + 1].Item1);
            }
            return result;
        }