Classes
class	vpRadarStation
class	vpACSimulator

Functions
float	normalize_angle (float angle)
ColVector	measurement_mean (List[ColVector] measurements, List[float] wm)
ColVector	measurementResidual (ColVector meas, ColVector to_subtract)
ColVector	state_add_vectors (a, b)
ColVector	state_residual_vectors (a, b)
ColVector	fx (ColVector x, float dt)
Matrix	generate_Q_matrix (float dt)
Matrix	generate_P0_matrix ()

Variables
bool	has_gui = True
int	dt = 3.
int	gt_X_init = -500.
int	gt_Y_init = 1000.
int	gt_vX_init = 100.
int	gt_vY_init = 5.
float	proc_var = 0.1
int	sigma_range = 5
	sigma_elev_angle = Math.rad(0.5)
float	stdev_aircraft_velocity = 0.2;
	drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=state_residual_vectors, addFunc=state_add_vectors)
	radar = vpRadarStation(0., 0., sigma_range, sigma_elev_angle)
Matrix	P0 = generate_P0_matrix()
	R = Matrix([[sigma_range * sigma_range, 0], [0, sigma_elev_angle * sigma_elev_angle]])
Matrix	Q = generate_Q_matrix(dt) * proc_var
	ukf = UnscentedKalman(Q, R, drawer, fx, radar.state_to_measurement)
int	num_plots = 4
	plot = Plot(num_plots)
list	plot_titles
list	plot_y_units
list	plot_legends = ['GT', 'Filtered']
	ac_pos = ColVector([gt_X_init, gt_Y_init])
	ac_vel = ColVector([gt_vX_init, gt_vY_init])
	ac = vpACSimulator(ac_pos, ac_vel, stdev_aircraft_velocity)
	gt_X_prev = ColVector([ac_pos[0], ac_pos[1]])
	gt_X = ac.update(dt)
tuple	gt_V = (gt_X - gt_X_prev) / dt
	z = radar.measure_with_noise(gt_X)
	Xest = ukf.getXest()

Function Documentation

◆ fx()

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

Process function that projects in time the internal state of the UKF.

: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 145 of file ukf-nonlinear-example.py.

◆ generate_P0_matrix()

Matrix ukf-nonlinear-example.generate_P0_matrix ( )

Method that generates the intial guess of the state covariance matrix.

@return Matrix The corresponding state covariance matrix.

Definition at line 270 of file ukf-nonlinear-example.py.

◆ generate_Q_matrix()

Matrix ukf-nonlinear-example.generate_Q_matrix ( float dt )

Method that generates the process covariance matrix for a process for which the
state vector can be written as (x, dx/dt)^T

:param dt: The sampling period.

:return Matrix: The corresponding process covariance matrix.

Definition at line 255 of file ukf-nonlinear-example.py.

◆ measurement_mean()

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

Compute the weighted mean of measurement vectors.

:param measurements: The measurement vectors, such as measurements[i][0] = range and
 measurements[i][1] = elevation_angle.
:param wm: The associated weights.

:return vpColVector: The weighted mean of the measurement vectors.

Definition at line 90 of file ukf-nonlinear-example.py.

◆ measurementResidual()

ColVector ukf-nonlinear-example.measurementResidual	(	ColVector	meas,
		ColVector	to_subtract )

Compute the subtraction between two vectors expressed in the measurement space,
 such as v[0] = range ; v[1] = elevation_angle

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

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

Definition at line 110 of file ukf-nonlinear-example.py.

References normalize_angle().

◆ normalize_angle()

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

Definition at line 82 of file ukf-nonlinear-example.py.

Referenced by measurementResidual().

◆ state_add_vectors()

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

Method that permits to add two state vectors.

: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 123 of file ukf-nonlinear-example.py.

◆ state_residual_vectors()

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

Method that permits to subtract a state vector to another.

: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 134 of file ukf-nonlinear-example.py.

Variable Documentation

◆ ac

ukf-nonlinear-example.ac = vpACSimulator(ac_pos, ac_vel, stdev_aircraft_velocity)

Definition at line 335 of file ukf-nonlinear-example.py.

◆ ac_pos

ukf-nonlinear-example.ac_pos = ColVector([gt_X_init, gt_Y_init])

Definition at line 333 of file ukf-nonlinear-example.py.

◆ ac_vel

ukf-nonlinear-example.ac_vel = ColVector([gt_vX_init, gt_vY_init])

Definition at line 334 of file ukf-nonlinear-example.py.

◆ drawer

ukf-nonlinear-example.drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=state_residual_vectors, addFunc=state_add_vectors)

Definition at line 295 of file ukf-nonlinear-example.py.

◆ dt

int ukf-nonlinear-example.dt = 3.

Definition at line 283 of file ukf-nonlinear-example.py.

◆ gt_V

tuple ukf-nonlinear-example.gt_V = (gt_X - gt_X_prev) / dt

Definition at line 340 of file ukf-nonlinear-example.py.

◆ gt_vX_init

int ukf-nonlinear-example.gt_vX_init = 100.

Definition at line 286 of file ukf-nonlinear-example.py.

◆ gt_vY_init

int ukf-nonlinear-example.gt_vY_init = 5.

Definition at line 287 of file ukf-nonlinear-example.py.

◆ gt_X

ukf-nonlinear-example.gt_X = ac.update(dt)

Definition at line 339 of file ukf-nonlinear-example.py.

◆ gt_X_init

int ukf-nonlinear-example.gt_X_init = -500.

Definition at line 284 of file ukf-nonlinear-example.py.

◆ gt_X_prev

ukf-nonlinear-example.gt_X_prev = ColVector([ac_pos[0], ac_pos[1]])

Definition at line 336 of file ukf-nonlinear-example.py.

◆ gt_Y_init

int ukf-nonlinear-example.gt_Y_init = 1000.

Definition at line 285 of file ukf-nonlinear-example.py.

◆ has_gui

bool ukf-nonlinear-example.has_gui = True

Definition at line 77 of file ukf-nonlinear-example.py.

◆ num_plots

int ukf-nonlinear-example.num_plots = 4

Definition at line 312 of file ukf-nonlinear-example.py.

◆ P0

Matrix ukf-nonlinear-example.P0 = generate_P0_matrix()

Definition at line 300 of file ukf-nonlinear-example.py.

◆ plot

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

Definition at line 313 of file ukf-nonlinear-example.py.

◆ plot_legends

list ukf-nonlinear-example.plot_legends = ['GT', 'Filtered']

Definition at line 322 of file ukf-nonlinear-example.py.

◆ plot_titles

list ukf-nonlinear-example.plot_titles

Initial value:

=  [
      'Position along X-axis', 'Velocity along X-axis',
      'Position along Y-axis', 'Velocity along Y-axis'
    ]

Definition at line 314 of file ukf-nonlinear-example.py.

◆ plot_y_units

list ukf-nonlinear-example.plot_y_units

Initial value:

=  [
      'Position (m)', 'Velocity (m/s)',
      'Position (m)', 'Velocity (m/s)'
    ]

Definition at line 318 of file ukf-nonlinear-example.py.

◆ proc_var

float ukf-nonlinear-example.proc_var = 0.1

Definition at line 288 of file ukf-nonlinear-example.py.

◆ Q

Matrix ukf-nonlinear-example.Q = generate_Q_matrix(dt) * proc_var

Definition at line 302 of file ukf-nonlinear-example.py.

◆ R

ukf-nonlinear-example.R = Matrix([[sigma_range * sigma_range, 0], [0, sigma_elev_angle * sigma_elev_angle]])

Definition at line 301 of file ukf-nonlinear-example.py.

◆ radar

ukf-nonlinear-example.radar = vpRadarStation(0., 0., sigma_range, sigma_elev_angle)

Definition at line 298 of file ukf-nonlinear-example.py.

◆ sigma_elev_angle

ukf-nonlinear-example.sigma_elev_angle = Math.rad(0.5)

Definition at line 290 of file ukf-nonlinear-example.py.

◆ sigma_range

int ukf-nonlinear-example.sigma_range = 5

Definition at line 289 of file ukf-nonlinear-example.py.

◆ stdev_aircraft_velocity

float ukf-nonlinear-example.stdev_aircraft_velocity = 0.2;

Definition at line 291 of file ukf-nonlinear-example.py.

◆ ukf

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

Definition at line 303 of file ukf-nonlinear-example.py.

◆ Xest

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

Definition at line 347 of file ukf-nonlinear-example.py.

◆ z

ukf-nonlinear-example.z = radar.measure_with_noise(gt_X)

Definition at line 341 of file ukf-nonlinear-example.py.

Classes

Functions

Variables