Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
ukf-linear-example Namespace Reference

Functions

ColVector fx (ColVector x, float dt)
ColVector hx (ColVector x)
ColVector add_state_vectors (a, b)
ColVector residual_state_vectors (a, b)
Matrix generate_Q_matrix (float dt)

Variables

bool has_gui = True
float dt = 0.01
float gt_dx = 0.01
float gt_dy = 0.005
 gt_dX = ColVector([gt_dx, gt_dy])
float gt_vx = gt_dx / dt
float gt_vy = gt_dy / dt
float proc_var = 0.000004
float sigma_x_meas = 0.05
float sigma_y_meas = 0.05
 drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors)
 P0 = Matrix(np.eye(4) * 1.)
 R = Matrix(np.eye(2) * 0.01)
Matrix Q = generate_Q_matrix(dt) * proc_var
 ukf = UnscentedKalman(Q, R, drawer, fx, hx)
int num_plots = 4
 plot = Plot(num_plots)
list plot_titles
list plot_y_units
list plot_legends = ['GT', 'Measure', 'Filtered']
 gt_X = ColVector(2, 0.)
 z_prec = ColVector(2, 0.)
 x_meas = gt_X[0] + np.random.normal(0.0, sigma_x_meas)
 y_meas = gt_X[1] + np.random.normal(0.0, sigma_y_meas)
 z = ColVector([x_meas, y_meas])
 Xest = ukf.getXest()
tuple vX_meas = (x_meas - z_prec[0]) / dt
tuple vY_meas = (y_meas - z_prec[1]) / dt

Function Documentation

◆ add_state_vectors()

ColVector ukf-linear-example.add_state_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 105 of file ukf-linear-example.py.

◆ fx()

ColVector ukf-linear-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 75 of file ukf-linear-example.py.

◆ generate_Q_matrix()

Matrix ukf-linear-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 127 of file ukf-linear-example.py.

◆ hx()

ColVector ukf-linear-example.hx ( ColVector x) 
Measurement function that expresses the internal state of the UKF in the measurement space.

:param x: The internal state of the UKF.

:return ColVector: The internal state, expressed in the measurement space.

Definition at line 92 of file ukf-linear-example.py.

◆ residual_state_vectors()

ColVector ukf-linear-example.residual_state_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 116 of file ukf-linear-example.py.

Variable Documentation

◆ drawer

ukf-linear-example.drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors)

Definition at line 154 of file ukf-linear-example.py.

◆ dt

float ukf-linear-example.dt = 0.01

Definition at line 143 of file ukf-linear-example.py.

◆ gt_dX

ukf-linear-example.gt_dX = ColVector([gt_dx, gt_dy])

Definition at line 146 of file ukf-linear-example.py.

◆ gt_dx

float ukf-linear-example.gt_dx = 0.01

Definition at line 144 of file ukf-linear-example.py.

◆ gt_dy

float ukf-linear-example.gt_dy = 0.005

Definition at line 145 of file ukf-linear-example.py.

◆ gt_vx

float ukf-linear-example.gt_vx = gt_dx / dt

Definition at line 147 of file ukf-linear-example.py.

◆ gt_vy

float ukf-linear-example.gt_vy = gt_dy / dt

Definition at line 148 of file ukf-linear-example.py.

◆ gt_X

ukf-linear-example.gt_X = ColVector(2, 0.)

Definition at line 187 of file ukf-linear-example.py.

◆ has_gui

bool ukf-linear-example.has_gui = True

Definition at line 70 of file ukf-linear-example.py.

◆ num_plots

int ukf-linear-example.num_plots = 4

Definition at line 166 of file ukf-linear-example.py.

◆ P0

ukf-linear-example.P0 = Matrix(np.eye(4) * 1.)

Definition at line 156 of file ukf-linear-example.py.

◆ plot

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

Definition at line 167 of file ukf-linear-example.py.

◆ plot_legends

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

Definition at line 176 of file ukf-linear-example.py.

◆ plot_titles

list ukf-linear-example.plot_titles
Initial value:
1= [
2 'Position along X-axis', 'Velocity along X-axis',
3 'Position along Y-axis', 'Velocity along Y-axis'
4 ]

Definition at line 168 of file ukf-linear-example.py.

◆ plot_y_units

list ukf-linear-example.plot_y_units
Initial value:
1= [
2 'Position (m)', 'Velocity (m/s)',
3 'Position (m)', 'Velocity (m/s)'
4 ]

Definition at line 172 of file ukf-linear-example.py.

◆ proc_var

float ukf-linear-example.proc_var = 0.000004

Definition at line 149 of file ukf-linear-example.py.

◆ Q

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

Definition at line 158 of file ukf-linear-example.py.

◆ R

ukf-linear-example.R = Matrix(np.eye(2) * 0.01)

Definition at line 157 of file ukf-linear-example.py.

◆ sigma_x_meas

float ukf-linear-example.sigma_x_meas = 0.05

Definition at line 150 of file ukf-linear-example.py.

◆ sigma_y_meas

float ukf-linear-example.sigma_y_meas = 0.05

Definition at line 151 of file ukf-linear-example.py.

◆ ukf

ukf-linear-example.ukf = UnscentedKalman(Q, R, drawer, fx, hx)

Definition at line 159 of file ukf-linear-example.py.

◆ vX_meas

tuple ukf-linear-example.vX_meas = (x_meas - z_prec[0]) / dt

Definition at line 204 of file ukf-linear-example.py.

◆ vY_meas

tuple ukf-linear-example.vY_meas = (y_meas - z_prec[1]) / dt

Definition at line 205 of file ukf-linear-example.py.

◆ x_meas

ukf-linear-example.x_meas = gt_X[0] + np.random.normal(0.0, sigma_x_meas)

Definition at line 193 of file ukf-linear-example.py.

◆ Xest

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

Definition at line 201 of file ukf-linear-example.py.

◆ y_meas

ukf-linear-example.y_meas = gt_X[1] + np.random.normal(0.0, sigma_y_meas)

Definition at line 194 of file ukf-linear-example.py.

◆ z

ukf-linear-example.z = ColVector([x_meas, y_meas])

Definition at line 195 of file ukf-linear-example.py.

◆ z_prec

ukf-linear-example.z_prec = ColVector(2, 0.)

Definition at line 188 of file ukf-linear-example.py.