private void initializeKalman() { float accelNoise = (float)numProcessNoise.Value; float measurementNoise = (float)numMeasurementNoise.Value; var measurementDimension = 2; //just coordinates var initialState = process.GetNoisyState(accelNoise); //assuming we measured process params (noise) var initialStateError = ModelState.GetProcessNoise(accelNoise); kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError, measurementDimension /*(position)*/, 0 /*no control*/, x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y }); kalman.ProcessNoise = ModelState.GetProcessNoise(accelNoise); kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, measurementNoise).ElementwisePower(2); //assuming we measured process params (noise) - ^2 => variance kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel) { //X, vX, Y, vY (look at ConstantVelocity2DModel) { 1, 0, 0, 0 }, //picks X { 0, 0, 1, 0 } //picks Y }; kalman.TransitionMatrix = ModelState.GetTransitionMatrix(ConstantVelocityProcess.TimeInterval); }
public void GoToNextState(out bool doneFullCycle) { Func<PointF, bool> isBorder = (point) => { return point.X <= 0 || point.X >= WorkingArea.Width || point.Y <= 0 || point.Y >= WorkingArea.Height; }; doneFullCycle = false; var prevPos = currentState.Position; var speed = currentState.Velocity; if (isBorder(currentState.Position)) { var temp = speed.X; speed.X = -speed.Y; speed.Y = temp; if (speed.Equals(initialState.Velocity)) doneFullCycle = true; } var nextState = new ConstantVelocity2DModel { Position = new PointF { X = prevPos.X + speed.X * TimeInterval, Y = prevPos.Y + speed.Y * TimeInterval }, Velocity = speed }; currentState = nextState; }
private void initializeKalman(PointF startPoint) { var measurementDimension = 2; //just coordinates var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.5f, -2f) }; var initialStateError = ModelState.GetProcessNoise(0); kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError, measurementDimension /*(position)*/, 0 /*no control*/, x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y }); kalman.ProcessNoise = ModelState.GetProcessNoise(2); kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 1); kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel) { //X, vX, Y, vY (look at ConstantVelocity2DModel) { 1, 0, 0, 0 }, //picks X { 0, 0, 1, 0 } //picks Y }; kalman.TransitionMatrix = ModelState.GetTransitionMatrix(); }
public ConstantVelocityProcess() { currentState = new ConstantVelocity2DModel { Position = new PointF(50, 1), Velocity = new PointF(0.3f, 0.3f) }; initialState = currentState; }
public static double[] ToArray(ConstantVelocity2DModel modelState) { return new double[] { modelState.Position.X, modelState.Velocity.X, modelState.Position.Y, modelState.Velocity.Y, }; }
/// <summary> /// Converts the model to the array. /// </summary> /// <param name="modelState">Model to convert.</param> /// <returns>Array.</returns> public static double[] ToArray(ConstantVelocity2DModel modelState) { return(new double[] { modelState.Position.X, modelState.Velocity.X, modelState.Position.Y, modelState.Velocity.Y, }); }
private void initializeKalman(PointF startPoint) { var measurementDimension = 2; //just coordinates var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.2f, -1.5f)}; var initialStateError = ModelState.GetProcessNoise(10); kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError, measurementDimension /*(position)*/, 0 /*no control*/, x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y }); kalman.ProcessNoise = ModelState.GetProcessNoise(1); kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 10000.0); kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix(); kalman.TransitionMatrix = ModelState.GetTransitionMatrix(); }
private void initializeKalman(PointF startPoint) { var measurementDimension = 2; //just coordinates var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.2f, -1.5f) }; var initialStateError = ModelState.GetProcessNoise(10); kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError, measurementDimension /*(position)*/, 0 /*no control*/, x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y }); kalman.ProcessNoise = ModelState.GetProcessNoise(1); kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 10000.0); kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix(); kalman.TransitionMatrix = ModelState.GetTransitionMatrix(); }
private void initializeKalman(PointF startPoint) { var measurementDimension = 2; //just coordinates var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.5f, -2f)}; var initialStateError = ModelState.GetProcessNoise(0); kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError, measurementDimension /*(position)*/, 0 /*no control*/, x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y }); kalman.ProcessNoise = ModelState.GetProcessNoise(2); kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 1); kalman.MeasurementMatrix = new double[,] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel) { //X, vX, Y, vY (look at ConstantVelocity2DModel) {1, 0, 0, 0}, //picks X {0, 0, 1, 0} //picks Y }; kalman.TransitionMatrix = ModelState.GetTransitionMatrix(); }
public static ConstantVelocity2DModel Evaluate(ConstantVelocity2DModel state, double[,] transitionMat) { var stateVector = state.ToArray().Multiply(transitionMat); return ConstantVelocity2DModel.FromArray(stateVector); }
/// <summary> /// Evaluates the model by using the provided transition matrix. /// </summary> /// <param name="transitionMat">Transition matrix.</param> /// <returns>New model state.</returns> public ConstantVelocity2DModel Evaluate(double[,] transitionMat) { var stateVector = transitionMat.Multiply(ToArray(this)); return(ConstantVelocity2DModel.FromArray(stateVector)); }
public static ConstantVelocity2DModel Evaluate(ConstantVelocity2DModel state, double[,] transitionMat) { var stateVector = state.ToArray().Multiply(transitionMat); return(ConstantVelocity2DModel.FromArray(stateVector)); }