public StepOutput <GPSFilter2DSample> Filter(GPSObservation obs, GPSFilter2DInput input) { var inputFromPrev = new StepInput <Matrix>(_prevEstimate); StepOutput <Matrix> result = CalculateNext(inputFromPrev, obs, input); _prevEstimate = result; return(ToFilterResult(result)); }
// 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, }); }
private static Matrix ToInputMatrix(GPSFilter2DInput input) { return(Matrix.Create(new double[p, 1] { // {0}, // Px // {0}, // Py // {0}, // Pz { input.Velocity.X }, // Vx { input.Velocity.Y }, // Vy { input.Velocity.Z }, // Vz { input.Acceleration.X }, // Ax { input.Acceleration.Y }, // Ay { input.Acceleration.Z }, // Az })); }
private static Matrix ToInputMatrix(GPSFilter2DInput input) { return Matrix.Create(new double[p,1] { // {0}, // Px // {0}, // Py // {0}, // Pz {input.Velocity.X}, // Vx {input.Velocity.Y}, // Vy {input.Velocity.Z}, // Vz {input.Acceleration.X}, // Ax {input.Acceleration.Y}, // Ay {input.Acceleration.Z}, // Az }); }
// 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, }; }
public StepOutput<GPSFilter2DSample> Filter(GPSObservation obs, GPSFilter2DInput input) { var inputFromPrev = new StepInput<Matrix>(_prevEstimate); StepOutput<Matrix> result = CalculateNext(inputFromPrev, obs, input); _prevEstimate = result; return ToFilterResult(result); }