예제 #1
0
        Predict(
            Matrix F
            )
        {
            KalmanFilter.CheckPredictParameters(F, this);

            this.x = F * this.x;
            this.P = (F * P * Matrix.Transpose(F));
        }
예제 #2
0
        DiscreteKalmanFilter(
            Matrix x0,
            Matrix P0
            )
        {
            KalmanFilter.CheckInitialParameters(x0, P0);

            this.x = x0;
            this.P = P0;
        }
예제 #3
0
        Predict(
            Matrix F,
            Matrix Q
            )
        {
            KalmanFilter.CheckPredictParameters(F, Q, this);

            // Predict the state
            this.x = F * x;
            this.P = (F * P * Matrix.Transpose(F)) + Q;
        }
        InformationFilter(
            Matrix x0,
            Matrix P0
            )
        {
            KalmanFilter.CheckInitialParameters(x0, P0);

            this.J = P0.Inverse();
            this.y = this.J * x0;
            this.I = Matrix.Identity(this.y.RowCount, this.y.RowCount);
        }
        Predict(
            Matrix F
            )
        {
            KalmanFilter.CheckPredictParameters(F, this);

            Matrix tmpG = new Matrix(this.x.RowCount, 1, 0.0d);
            Matrix tmpQ = new Matrix(1, 1, 1.0d);

            this.Predict(F, tmpG, tmpQ);
        }
예제 #6
0
        Predict(
            Matrix F,
            Matrix G,
            Matrix Q
            )
        {
            KalmanFilter.CheckPredictParameters(F, G, Q, this);

            // State prediction
            this.x = F * x;
            // Covariance update
            this.P = (F * P * Matrix.Transpose(F)) + (G * Q * Matrix.Transpose(G));
        }
        SquareRootFilter(
            Matrix x0,
            Matrix P0
            )
        {
            KalmanFilter.CheckInitialParameters(x0, P0);

            // Decompose the covariance matrix
            Matrix[] UDU = UDUDecomposition(P0);
            this.U = UDU[0];
            this.D = UDU[1];
            this.x = x0;
        }
        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);
        }
        Predict(
            Matrix F
            )
        {
            KalmanFilter.CheckPredictParameters(F, this);

            // Easier just to convert back to discrete form....
            Matrix P = this.J.Inverse();
            Matrix x = P * this.y;

            x = F * x;
            P = F * P * Matrix.Transpose(F);

            this.J = P.Inverse();
            this.y = this.J * x;
        }
예제 #10
0
        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
        }
        InformationFilter(
            Matrix state,
            Matrix cov,
            bool inverted
            )
        {
            KalmanFilter.CheckInitialParameters(state, cov);

            if (inverted)
            {
                this.J = cov;
                this.y = state;
            }
            else
            {
                this.J = cov.Inverse();
                this.y = this.J * state;
            }
            this.I = Matrix.Identity(state.RowCount, state.RowCount);
        }
예제 #12
0
        Predict(
            Matrix F,
            Matrix G,
            Matrix Q
            )
        {
            KalmanFilter.CheckPredictParameters(F, G, Q, this);

            // Update the state.. that is easy!!
            this.x = F * this.x;

            // Get all the sized and create storage
            int    n    = this.x.RowCount;
            int    p    = G.ColumnCount;
            Matrix outD = new Matrix(n, n);      // Updated diagonal matrix
            Matrix outU = Matrix.Identity(n, n); // Updated upper unit triangular

            // Get the UD Decomposition of the process noise matrix
            Matrix[] UDU = UDUDecomposition(Q);
            Matrix   Uq  = UDU[0];
            Matrix   Dq  = UDU[1];

            // Combine it with the noise coupling matrix
            Matrix Gh = G * Uq;

            // Convert state transition matrix
            Matrix PhiU = F * (new Matrix(this.U));

            // Ready to go..
            for (int i = n - 1; i >= 0; i--)
            {
                // Update the i'th diagonal of the covariance matrix
                double sigma = 0.0d;
                for (int j = 0; j < n; j++)
                {
                    sigma = sigma + (PhiU[i, j] * PhiU[i, j] * this.D[j, j]);
                }
                for (int j = 0; j < p; j++)
                {
                    sigma = sigma + (Gh[i, j] * Gh[i, j] * Dq[j, j]);
                }
                outD[i, i] = sigma;
                // Update the i'th row of the upper triangular of covariance
                for (int j = 0; j < i; j++)
                {
                    sigma = 0.0d;
                    for (int k = 0; k < n; k++)
                    {
                        sigma = sigma + (PhiU[i, k] * this.D[k, k] * PhiU[j, k]);
                    }
                    for (int k = 0; k < p; k++)
                    {
                        sigma = sigma + (Gh[i, k] * Dq[k, k] * Gh[j, k]);
                    }
                    outU[j, i] = sigma / outD[i, i];
                    for (int k = 0; k < n; k++)
                    {
                        PhiU[j, k] = PhiU[j, k] - (outU[j, i] * PhiU[i, k]);
                    }
                    for (int k = 0; k < p; k++)
                    {
                        Gh[j, k] = Gh[j, k] - (outU[j, i] * Gh[i, k]);
                    }
                }
            }
            // Update the covariance
            this.U = outU;
            this.D = outD;
        }