/// <summary> /// Creates an Information filter with the given initial state. /// </summary> /// <param name="x0">Initial estimate of state variables.</param> /// <param name="P0">Covariance of state variable estimates.</param> public InformationFilter(Matrix<double> x0, Matrix<double> P0) { KalmanFilter.CheckInitialParameters(x0, P0); J = P0.Inverse(); y = J*x0; I = Matrix<double>.Build.DenseIdentity(y.RowCount, y.RowCount); }
// Observations are never used. // public IEnumerable<StepOutput<GPSFilter2DSample>> Filter(IEnumerable<GPSObservation> samples, IEnumerable<GPSFilter2DInput> inputs) // { // return samples.Select(s => Filter(s, i)); // } private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSObservation observation, GPSFilter2DInput input) { TimeSpan time = observation.Time; TimeSpan timeDelta = time - _prevEstimate.Time; UpdateMatrices(time, timeDelta); Matrix u = ToInputMatrix(input); // Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter // Calculate the state and the output Matrix x = A * prev.X + B * u + w; Matrix z = H * x + v; //ToOutputMatrix(observation) + v;//H * x + v; // Predictor equations Matrix PriX = A * prev.PostX + B * u; // n by 1 Matrix AT = Matrix.Transpose(A); Matrix PriP = A * prev.PostP * AT + Q; // n by n Matrix residual = z - H * PriX; Matrix residualP = H * PriP * Matrix.Transpose(H) + R; Matrix residualPInv = residualP.Inverse(); // Corrector equations Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m // TODO Temp, experimenting with skipping measurements // compileerror // k[PositionY, PositionY] = 1.0; // k[VelocityY, VelocityY] = 1.0; // k[AccelerationY, AccelerationY] = 1.0; Matrix PostX = PriX + K * residual; // n by 1 Matrix PostP = (I - K * H) * PriP; // n by n return(new StepOutput <Matrix> { K = K, PriX = PriX, PriP = PriP, PostX = PostX, PostP = PostP, PostXError = PostX - x, PriXError = PriX - x, X = x, Z = z, W = w, V = v, Time = time, }); }
/// <summary> /// Creates an Information filter specifying whether the covariance and state /// have been 'inverted'. /// </summary> /// <param name="state">The initial estimate of the state of the system.</param> /// <param name="cov">The covariance of the initial state estimate.</param> /// <param name="inverted">Has covariance/state been converted to information /// filter form?</param> /// <remarks>This behaves the same as other constructors if the given boolean is false. /// Otherwise, in relation to the given state/covariance should satisfy:<BR></BR> /// <C>cov = J = P0 ^ -1, state = y = J * x0.</C></remarks> public InformationFilter(Matrix<double> state, Matrix<double> cov, bool inverted) { KalmanFilter.CheckInitialParameters(state, cov); if (inverted) { J = cov; y = state; } else { J = cov.Inverse(); y = J*state; } I = Matrix<double>.Build.DenseIdentity(state.RowCount, state.RowCount); }
/// <summary> /// Updates the state of the system based on the given noisy measurements, /// a description of how those measurements relate to the system, and a /// covariance <c>Matrix</c> to describe the noise of the system. /// </summary> /// <param name="z">The measurements of the system.</param> /// <param name="H">Measurement model.</param> /// <param name="R">Covariance of measurements.</param> /// <exception cref="System.ArgumentException">Thrown when given matrices /// are of the incorrect size.</exception> public void Update(Matrix<double> z, Matrix<double> H, Matrix<double> R) { KalmanFilter.CheckUpdateParameters(z, H, R, this); // Fiddle with the matrices Matrix<double> HT = H.Transpose(); Matrix<double> RI = R.Inverse(); // Perform the update y = y + (HT*RI*z); J = J + (HT*RI*H); }
/// <summary> /// Perform a discrete time prediction of the system state. /// </summary> /// <param name="F">State transition matrix.</param> /// <param name="G">Noise coupling matrix.</param> /// <param name="Q">Plant noise covariance.</param> /// <exception cref="System.ArgumentException">Thrown when the column and row /// counts for the given matrices are incorrect.</exception> /// <remarks> /// Performs a prediction of the next state of the Kalman Filter, given /// a description of the dynamic equations of the system, the covariance of /// the plant noise affecting the system and the equations that describe /// the effect on the system of that plant noise. /// </remarks> public void Predict(Matrix<double> F, Matrix<double> G, Matrix<double> Q) { // Some matrices we will need a bit Matrix<double> FI = F.Inverse(); Matrix<double> FIT = FI.Transpose(); Matrix<double> GT = G.Transpose(); Matrix<double> A = FIT*J*FI; Matrix<double> B = A*G*(GT*A*G + Q.Inverse()).Inverse(); J = (I - B*GT)*A; y = (I - B*GT)*FIT*y; }
/// <summary> /// Preform a discrete time prediction of the system state. /// </summary> /// <param name="F">State transition matrix.</param> /// <param name="Q">A plant noise covariance matrix.</param> /// <exception cref="System.ArgumentException">Thrown when F and Q are not /// square matrices with the same number of rows and columns as there are /// rows in the state matrix.</exception> /// <remarks>Performs a prediction of the next state of the Kalman Filter, /// where there is plant noise. The covariance matrix of the plant noise, in /// this case, is a square matrix corresponding to the state transition and /// the state of the system.</remarks> public void Predict(Matrix<double> F, Matrix<double> Q) { // We will need these matrices more than once... Matrix<double> FI = F.Inverse(); Matrix<double> FIT = FI.Transpose(); Matrix<double> A = FIT*J*FI; Matrix<double> AQI = (A + Q.Inverse()).Inverse(); // 'Covariance' Update J = A - (A*AQI*A); y = (I - (A*AQI))*FIT*y; }
private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSINSObservation observation, GPSINSInput input) { TimeSpan time = observation.Time; TimeSpan timeDelta = time - _prevEstimate.Time; // Note: Use 0.0 and not 0 in order to use the correct constructor! // If no GPS update is available, then use a zero matrix to ignore the values in the observation. H = observation.GotGPSUpdate ? Matrix.Identity(m, n) : new Matrix(m, n, 0.0); HT = Matrix.Transpose(H); UpdateTimeVaryingMatrices(timeDelta); Matrix u = ToInputMatrix(input, time); // Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter // Calculate the state and the output Matrix x = A * prev.X + B * u; // +w; noise is already modelled in input data Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1 // Predictor equations Matrix PriX = A * prev.PostX + B * u; // n by 1 Matrix PriP = A * prev.PostP * AT + Q; // n by n Matrix residual = z - H * PriX; // m by 1 Matrix residualP = H * PriP * HT + R; // m by m // If residualP is zero matrix then set its inverse to zero as well. // This occurs if all observation standard deviations are zero // and this workaround will cause the Kalman gain to trust the process model entirely. Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m ? _zeroMM : residualP.Inverse(); // Corrector equations Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m Matrix PostX = PriX + K * residual; // n by 1 Matrix PostP = (I - K * H) * PriP; // n by n var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]); // var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]); // Vector3 positionChange = tmpPosition - tmpPrevPosition; Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]); Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]); // // Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q; var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]); var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]); return(new StepOutput <Matrix> { K = K, PriX = PriX, PriP = PriP, PostX = PostX, PostP = PostP, PostXError = PostX - x, PriXError = PriX - x, X = x, Z = z, W = w, V = v, Time = time, }); }