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, }); }
/// <summary> /// Transform sensor measurements to output matrix /// </summary> /// <param name="s"></param> /// <returns></returns> private static Matrix ToOutputMatrix(GPSObservation s) { return(Matrix.Create(new double[m, 1] { { s.Position.X }, { s.Position.Y }, { s.Position.Z }, // {0}, {0}, {0}, // Velocity // {0}, {0}, {0}, // Acceleration // {s.Time.TotalSeconds}, })); }
/// <summary></summary> /// <param name="x0">Initial system state</param> /// <param name="postX0"> /// Initial guess of system state /// </param> /// <param name="postP0"> /// Initial guess of a posteriori covariance /// </param> /// <returns></returns> private StepOutput <Matrix> GetInitialEstimate(Matrix x0, Matrix postX0, Matrix postP0) { var startingCondition = new StepInput <Matrix>(x0, postX0, postP0); // TODO Is it ok to use an observation for the initial state in this manner? var observation = new GPSObservation { Position = GetPosition(x0), Time = TimeSpan.Zero, //GetTime(x0) }; return(CalculateNext(startingCondition, observation, new GPSFilter2DInput())); }
/// <summary></summary> public void GPSFilter2DTest() { try { if (Chart == null) return; // Add test Lines to demonstrate the control Chart.Primitives.Clear(); var filterStartState = new GPSObservation { Position = new Vector3(0, 0, 0), // Velocity = new Vector3(1, 0, 0), // Acceleration = new Vector3(0, 1, 0), Time = TimeSpan.Zero, }; var filter = new GPSFilter2D(filterStartState); var inValues = new List<GPSObservation>(); for (int t = 0; t < 20; t++) { var obs = new GPSObservation { Position = new Vector3(t, 2.5f*0.01f*t*t, 0), //(float) (1 + 1*Math.Cos(2*Math.PI*4*t/100)), 0), Time = TimeSpan.FromSeconds(t) }; inValues.Add(obs); } var outValues = new List<StepOutput<GPSFilter2DSample>>(); foreach (GPSObservation observation in inValues) outValues.Add(filter.Filter(observation, new GPSFilter2DInput { Velocity = new Vector3(1, 0, 0), Acceleration = new Vector3(0, 0.01f, 0), })); // IEnumerable<StepOutput<GPSFilter2DSample>> outValues = filter.Filter(inValues); // AddLineXY("N-Window Smoothing", Colors.DarkGoldenrod, originalZValues);//smoothedZValues); // AddLineXY("Noise W", Colors.White, outValues.Select(element => element.W)); // AddLineXY("Noise V", Colors.LightGray, outValues.Select(e => e.V.Position)); // AddLineXY("k", Colors.Cyan, outValues.Select(element => element.K)); SwordfishGraphHelper.AddLineXY(Chart, "A Priori X", Colors.Blue, outValues.Select(e => e.PriX.Position)); SwordfishGraphHelper.AddLineXY(Chart, "A Posteriori X", Colors.YellowGreen, outValues.Select(e => e.PostX.Position)); SwordfishGraphHelper.AddLineXY(Chart, "X", Color.FromArgb(255, 150, 0, 0), outValues.Select(e => e.X.Position)); SwordfishGraphHelper.AddLineXY(Chart, "Z", Color.FromArgb(255, 255, 255, 0), outValues.Select(e => e.Z.Position)); // AddLineXY(Chart, "True", Color.FromArgb(50, 150, 0, 0), inValues.Select(e => e.Position)); Chart.Title = "Filter2DTest()"; Chart.XAxisLabel = "X [m]"; Chart.YAxisLabel = "Y [m]"; Chart.RedrawPlotLines(); } catch (Exception e) { MessageBox.Show(e.ToString()); } }
public GPSFilter2D(GPSObservation startState) { X0 = Matrix.Create(new double[n, 1] { { startState.Position.X }, { startState.Position.Y }, { startState.Position.Z }, // Position { 0 }, { 0 }, { 0 }, // Velocity { 0 }, { 0 }, { 0 }, // Acceleration }); PostX0 = X0.Clone(); /* Matrix.Create(new double[n, 1] * { * {startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position * {1}, {0}, {0}, // Velocity * {0}, {1}, {0}, // Acceleration * });*/ // Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components // have no correlation and behave independently. This not entirely true. PostP0 = Matrix.Identity(n, n); // Refs: // http://www.romdas.com/technical/gps/gps-acc.htm // http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf // http://onlinestatbook.com/java/normalshade.html // Assuming GPS Sensor: FV-M8 // Cold start: 41s // Hot start: 1s // Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth) // Position precision (DGPS): 2.6m CEP // const float coVarQ = ; // _r = covarianceR; // Determine standard deviation of estimated process and observation noise variance // Position process noise _stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q); _rand = new GaussianRandom(); // Circle Error Probable (50% of the values are within this radius) // const float cep = 3.3f; // GPS position observation noise by standard deviation [meters] // Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%) // ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm // Found empirically by http://onlinestatbook.com/java/normalshade.html // using area=0.5 and limits +- 3.3 meters _stdDevV = new Vector(new double[m] { 0, ObservationNoiseStdDevY, 0, // 0, // 0, // 0, // 0, // 0, // 0, // 0, }); //Vector.Zeros(Observations); //2, 2, 4.8926f); H = Matrix.Identity(m, n); I = Matrix.Identity(n, n); Q = new Matrix(n, n); R = Matrix.Identity(m, m); _prevEstimate = GetInitialEstimate(X0, PostX0, PostP0); }
// 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); }
/// <summary></summary> /// <param name="x0">Initial system state</param> /// <param name="postX0"> /// Initial guess of system state /// </param> /// <param name="postP0"> /// Initial guess of a posteriori covariance /// </param> /// <returns></returns> private StepOutput<Matrix> GetInitialEstimate(Matrix x0, Matrix postX0, Matrix postP0) { var startingCondition = new StepInput<Matrix>(x0, postX0, postP0); // TODO Is it ok to use an observation for the initial state in this manner? var observation = new GPSObservation { Position = GetPosition(x0), Time = TimeSpan.Zero, //GetTime(x0) }; return CalculateNext(startingCondition, observation, new GPSFilter2DInput()); }
/// <summary> /// Transform sensor measurements to output matrix /// </summary> /// <param name="s"></param> /// <returns></returns> private static Matrix ToOutputMatrix(GPSObservation s) { return Matrix.Create(new double[m,1] { {s.Position.X}, {s.Position.Y}, {s.Position.Z}, // {0}, {0}, {0}, // Velocity // {0}, {0}, {0}, // Acceleration // {s.Time.TotalSeconds}, }); }
public GPSFilter2D(GPSObservation startState) { X0 = Matrix.Create(new double[n,1] { {startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position {0}, {0}, {0}, // Velocity {0}, {0}, {0}, // Acceleration }); PostX0 = X0.Clone(); /* Matrix.Create(new double[n, 1] { {startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position {1}, {0}, {0}, // Velocity {0}, {1}, {0}, // Acceleration });*/ // Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components // have no correlation and behave independently. This not entirely true. PostP0 = Matrix.Identity(n, n); // Refs: // http://www.romdas.com/technical/gps/gps-acc.htm // http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf // http://onlinestatbook.com/java/normalshade.html // Assuming GPS Sensor: FV-M8 // Cold start: 41s // Hot start: 1s // Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth) // Position precision (DGPS): 2.6m CEP // const float coVarQ = ; // _r = covarianceR; // Determine standard deviation of estimated process and observation noise variance // Position process noise _stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q); _rand = new GaussianRandom(); // Circle Error Probable (50% of the values are within this radius) // const float cep = 3.3f; // GPS position observation noise by standard deviation [meters] // Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%) // ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm // Found empirically by http://onlinestatbook.com/java/normalshade.html // using area=0.5 and limits +- 3.3 meters _stdDevV = new Vector(new double[m] { 0, ObservationNoiseStdDevY, 0, // 0, // 0, // 0, // 0, // 0, // 0, // 0, }); //Vector.Zeros(Observations); //2, 2, 4.8926f); H = Matrix.Identity(m, n); I = Matrix.Identity(n, n); Q = new Matrix(n, n); R = Matrix.Identity(m, m); _prevEstimate = GetInitialEstimate(X0, PostX0, PostP0); }