Classes
class	vpBicycleModel
class	LandmarkMeasurements
class	LandmarksGrid

Functions
float	normalize_angle (float angle)
ColVector	measurement_mean (List[ColVector] measurements, List[float] wm)
ColVector	measurement_residual (ColVector meas, ColVector to_subtract)
ColVector	state_add_vectors (ColVector a, ColVector b)
ColVector	state_mean_vectors (List[ColVector] states, List[float] wm)
ColVector	state_residual_vectors (a, b)
ColVector	fx (ColVector x, float dt)
List[ColVector]	generate_turn_commands (float v, float angleStart, float angleStop, int nbSteps)
List[ColVector]	generate_commands ()

Variables
bool	has_gui = True
float	dt = 0.1
int	step = 1.
float	sigma_range = 0.3
	sigma_bearing = Math.rad(0.5)
float	wheelbase = 0.5
float	process_variance = 0.0001
list	positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)]
list	landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions]
	nbLandmarks = len(landmarks)
List[ColVector]	cmds = generate_commands()
	nb_cmds = len(cmds)
	grid = LandmarksGrid(landmarks)
	robot = vpBicycleModel(wheelbase)
	drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=state_residual_vectors, addFunc=state_add_vectors)
	P0
	R1landmark = Matrix([[sigma_range * sigma_range, 0], [0, sigma_bearing*sigma_bearing]])
	R = Matrix(2nbLandmarks, 2 nbLandmarks)
	Q = Matrix()
	X0 = ColVector([2., 6., 0.3])
	ukf = UnscentedKalman(Q, R, drawer, fx, grid.state_to_measurement)
int	num_plots = 1
	plot = Plot(num_plots)
	robot_pos = X0
	z = grid.measure_with_noise(robot_pos)
	Xest = ukf.getXest()

Function Documentation

◆ fx()

ColVector ukf-nonlinear-complex-example.fx	(	ColVector	x,
		float	dt )

As the state model {x, y, \f$ \theta \f$} does not contain any velocity
information, it does not evolve without commands.

:param x: The internal state of the UKF.
:param dt: The sampling time: how far in the future are we projecting x.

:return ColVector: The updated internal state, projected in time, also known as the prior.

Definition at line 189 of file ukf-nonlinear-complex-example.py.

◆ generate_commands()

List[ColVector] ukf-nonlinear-complex-example.generate_commands ( )

Generate the list of commands for the simulation.

:return List[ColVector]: The list of commands to use in the simulation

Definition at line 219 of file ukf-nonlinear-complex-example.py.

References generate_turn_commands().

◆ generate_turn_commands()

List[ColVector] ukf-nonlinear-complex-example.generate_turn_commands	(	float	v,
		float	angleStart,
		float	angleStop,
		int	nbSteps )

Compute the commands realising a turn at constant linear velocity.

:param v: Constant linear velocity.
:param angleStart: Starting angle (in degrees).
:param angleStop: Stop angle (in degrees).
:param nbSteps: Number of steps to perform the turn.
:return List[ColVector]: The corresponding list of commands.

Definition at line 201 of file ukf-nonlinear-complex-example.py.

Referenced by generate_commands().

◆ measurement_mean()

ColVector ukf-nonlinear-complex-example.measurement_mean	(	List[ColVector]	measurements,
		List[float]	wm )

Compute the weighted mean of measurement vectors.

:param measurements: The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0;
  v[2] = dist_1 ; v[3] = bearing_1 ...
:param wm: The associated weights.

:return ColVector: The weighted mean.

Definition at line 102 of file ukf-nonlinear-complex-example.py.

◆ measurement_residual()

ColVector ukf-nonlinear-complex-example.measurement_residual	(	ColVector	meas,
		ColVector	to_subtract )

Compute the subtraction between two vectors expressed in the measurement space,
such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ...

:param meas: Measurement to which we must subtract something.
:param toSubtract: The something we must subtract.

:return ColVector: \b meas - \b toSubtract .

Definition at line 129 of file ukf-nonlinear-complex-example.py.

References normalize_angle().

◆ normalize_angle()

float ukf-nonlinear-complex-example.normalize_angle ( float angle )

Definition at line 94 of file ukf-nonlinear-complex-example.py.

Referenced by ukf-nonlinear-complex-example.LandmarkMeasurements.measure_gt(), ukf-nonlinear-complex-example.LandmarkMeasurements.measure_with_noise(), measurement_residual(), ukf-nonlinear-complex-example.vpBicycleModel.move(), state_add_vectors(), state_residual_vectors(), and ukf-nonlinear-complex-example.LandmarkMeasurements.state_to_measurement().

◆ state_add_vectors()

ColVector ukf-nonlinear-complex-example.state_add_vectors	(	ColVector	a,
		ColVector	b )

Compute the addition between two vectors expressed in the state space,
such as v[0] = x ; v[1] = y; v[2] = heading .

:param a: The first state vector to which another state vector must be added.
:param b: The other state vector that must be added to a.

:return ColVector: The sum a + b.

Definition at line 143 of file ukf-nonlinear-complex-example.py.

References normalize_angle().

◆ state_mean_vectors()

ColVector ukf-nonlinear-complex-example.state_mean_vectors	(	List[ColVector]	states,
		List[float]	wm )

Compute the weighted mean of state vectors.

:param states: The state vectors.
:param wm: The associated weights.
:return ColVector: The weighted mean.

Definition at line 157 of file ukf-nonlinear-complex-example.py.

◆ state_residual_vectors()

ColVector ukf-nonlinear-complex-example.state_residual_vectors	(		a,
			b )

Compute the subtraction between two vectors expressed in the state space,
such as v[0] = x ; v[1] = y; v[2] = heading .

:param a: The first state vector to which another state vector must be subtracted.
:param b: The other state vector that must be subtracted to a.

:return ColVector: The subtraction a - b.

Definition at line 176 of file ukf-nonlinear-complex-example.py.

References normalize_angle().

Variable Documentation

◆ cmds

List[ColVector] ukf-nonlinear-complex-example.cmds = generate_commands()

Definition at line 459 of file ukf-nonlinear-complex-example.py.

◆ drawer

ukf-nonlinear-complex-example.drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=state_residual_vectors, addFunc=state_add_vectors)

Definition at line 467 of file ukf-nonlinear-complex-example.py.

◆ dt

float ukf-nonlinear-complex-example.dt = 0.1

Definition at line 450 of file ukf-nonlinear-complex-example.py.

◆ grid

ukf-nonlinear-complex-example.grid = LandmarksGrid(landmarks)

Definition at line 463 of file ukf-nonlinear-complex-example.py.

◆ has_gui

bool ukf-nonlinear-complex-example.has_gui = True

Definition at line 89 of file ukf-nonlinear-complex-example.py.

◆ landmarks

list ukf-nonlinear-complex-example.landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions]

Definition at line 457 of file ukf-nonlinear-complex-example.py.

◆ nb_cmds

ukf-nonlinear-complex-example.nb_cmds = len(cmds)

Definition at line 460 of file ukf-nonlinear-complex-example.py.

◆ nbLandmarks

ukf-nonlinear-complex-example.nbLandmarks = len(landmarks)

Definition at line 458 of file ukf-nonlinear-complex-example.py.

◆ num_plots

int ukf-nonlinear-complex-example.num_plots = 1

Definition at line 497 of file ukf-nonlinear-complex-example.py.

◆ P0

ukf-nonlinear-complex-example.P0

Initial value:

=  Matrix([[0.1, 0., 0.],
               [0., 0.1, 0.],
               [0., 0., 0.05]])

Definition at line 470 of file ukf-nonlinear-complex-example.py.

◆ plot

ukf-nonlinear-complex-example.plot = Plot(num_plots)

Definition at line 498 of file ukf-nonlinear-complex-example.py.

◆ positions

list ukf-nonlinear-complex-example.positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)]

Definition at line 456 of file ukf-nonlinear-complex-example.py.

◆ process_variance

float ukf-nonlinear-complex-example.process_variance = 0.0001

Definition at line 455 of file ukf-nonlinear-complex-example.py.

◆ Q

float ukf-nonlinear-complex-example.Q = Matrix()

Definition at line 478 of file ukf-nonlinear-complex-example.py.

◆ R

ukf-nonlinear-complex-example.R = Matrix(2*nbLandmarks, 2 * nbLandmarks)

Definition at line 474 of file ukf-nonlinear-complex-example.py.

◆ R1landmark

ukf-nonlinear-complex-example.R1landmark = Matrix([[sigma_range * sigma_range, 0], [0, sigma_bearing*sigma_bearing]])

Definition at line 473 of file ukf-nonlinear-complex-example.py.

◆ robot

ukf-nonlinear-complex-example.robot = vpBicycleModel(wheelbase)

Definition at line 464 of file ukf-nonlinear-complex-example.py.

◆ robot_pos

ukf-nonlinear-complex-example.robot_pos = X0

Definition at line 506 of file ukf-nonlinear-complex-example.py.

◆ sigma_bearing

ukf-nonlinear-complex-example.sigma_bearing = Math.rad(0.5)

Definition at line 453 of file ukf-nonlinear-complex-example.py.

◆ sigma_range

float ukf-nonlinear-complex-example.sigma_range = 0.3

Definition at line 452 of file ukf-nonlinear-complex-example.py.

◆ step

int ukf-nonlinear-complex-example.step = 1.

Definition at line 451 of file ukf-nonlinear-complex-example.py.

◆ ukf

ukf-nonlinear-complex-example.ukf = UnscentedKalman(Q, R, drawer, fx, grid.state_to_measurement)

Definition at line 484 of file ukf-nonlinear-complex-example.py.

◆ wheelbase

float ukf-nonlinear-complex-example.wheelbase = 0.5

Definition at line 454 of file ukf-nonlinear-complex-example.py.

◆ X0

ukf-nonlinear-complex-example.X0 = ColVector([2., 6., 0.3])

Definition at line 481 of file ukf-nonlinear-complex-example.py.

◆ Xest

ukf-nonlinear-complex-example.Xest = ukf.getXest()

Definition at line 519 of file ukf-nonlinear-complex-example.py.

◆ z

ukf-nonlinear-complex-example.z = grid.measure_with_noise(robot_pos)

Definition at line 512 of file ukf-nonlinear-complex-example.py.

Classes

Functions

Variables

Function Documentation

◆ fx()

◆ generate_commands()

◆ generate_turn_commands()

◆ measurement_mean()

◆ measurement_residual()

◆ normalize_angle()

◆ state_add_vectors()

◆ state_mean_vectors()

◆ state_residual_vectors()

Variable Documentation

◆ cmds

◆ drawer

◆ dt

◆ grid

◆ has_gui

◆ landmarks

◆ nb_cmds

◆ nbLandmarks

◆ num_plots

◆ P0

◆ plot

◆ positions

◆ process_variance

◆ Q

◆ R

◆ R1landmark

◆ robot

◆ robot_pos

◆ sigma_bearing

◆ sigma_range

◆ step

◆ ukf

◆ wheelbase

◆ X0

◆ Xest

◆ z