Predict( Matrix F ) { KalmanFilter.CheckPredictParameters(F, this); this.x = F * this.x; this.P = (F * P * Matrix.Transpose(F)); }
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; }
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); }
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)); }
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; }
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; }