Smoothing

Smoothing#

During the smoothing stage, the noisy track positions of type PoseTrack are smoothed using the Rauch-Tung-Striebal (RTS) smoother. The output is a trajectory of type Trajectory.

Trajectory class#

class mona.smoothing.smoothing.Trajectory#

Vehicle trajectory representation as a time-discrete sequence of states.

traj_points: recarray#

length: float#

width: float#

traj_id: int#

vehicle_type: DetectionLabel = 2#

columns: ClassVar[List[str]] = ['frame_id', 'agent_type', 'x', 'y', 'v', 'psi_rad', 'yaw_rate', 'length', 'width', 'timestamp_sec']#

classmethod create_trajectory(track, x, y, speed, theta, timestamp, yaw_rate)#

Create a trajectory from separate state component arrays.

Parameters:

track (PoseTrack) –
x (ndarray) –
y (ndarray) –
speed (ndarray) –
theta (ndarray) –
timestamp (ndarray) –
yaw_rate (ndarray) –

Return type:

Trajectory

differentiate()#

Calculate and store velocity and orientation by finite differences.

Return type:: None

property dataframe: DataFrame#: Return vehicle trajectory as a pandas dataframe.

property start_time_step: int#: Return the start time step of the trajectory.

__init__(traj_points, length, width, traj_id, vehicle_type=DetectionLabel.car)#

Parameters:

traj_points (recarray) –
length (float) –
width (float) –
traj_id (int) –
vehicle_type (DetectionLabel) –

Return type:

None

`BaseSmoother` class#

The BaseSmoother defines the above mentioned input and output of the smoothing stage.

class mona.smoothing.smoothing.BaseSmoother#

Base smoother class that processes pose track.

__init__(dt, num_init_states, debug=False, output_dir='/tmp/')#

Initialize BaseSmoother class.

Parameters:

dt (float) – time increment of input data
num_init_states (int) – number of states the track must have to not be dropped
debug (bool) – flag that will trigger debug plots
output_dir (str) – output directory for debug plots

abstract smooth(track)#

Smooth respective track and return the smoothed trajectory.

Parameters:: track (PoseTrack) – Track to be smoothed
Return type:: Optional[Trajectory]

process_all_tracks(pose_tracks)#

Smooth all tracks.

Parameters:: pose_tracks (List[PoseTrack]) – List of tracks to be smoothed
Return type:: List[Trajectory]

debug_plots(track, pos_filtered, pos_smoothed)#

Plot debug information for raw, filtered, and smoothed trajectory.

Parameters:

track (PoseTrack) –
pos_filtered (ndarray) –
pos_smoothed (ndarray) –

Return type:

None

`ModelBasedRtsSmoother` class#

The ModelBasedRtsSmoother is a Model-based Rauch-Tung-Striebal (RTS) smoother, which extends BaseSmoother class and will process and smooth all pose tracks.

class mona.smoothing.smoothing.ModelBasedRtsSmoother#

Model-based Rauch-Tung-Striebal (RTS) Smoother.

Extends BaseSmoother class.

__init__(dt, num_init_states, config, orientation_estimator, debug=False, output_dir='/tmp/smoother')#

Initialize ModelBasedRtsSmoother class.

Parameters:

dt (float) – time increment of input data
num_init_states (int) – number of states the track must have to not be dropped
debug (bool) – flag that will trigger debug plots
output_dir (str) – output directory for debug plots
config (DictConfig) –
orientation_estimator (MapOrientationEstimator) –

create_and_initialize_filter(track)#

Create a filter object and initialize it.

Parameters:: track (PoseTrack) – Track to be smoothed
Return type:: BaseFilter

smooth(track)#

Smooth respective track and return the smoothed trajectory.

Parameters:: track (PoseTrack) – Track to be smoothed
Return type:: Optional[Trajectory]

`BaseFilter` class#

ModelBasedRtsSmoother requires an underlying filter implementation, which is defined by BaseFilter. .. autoclass:: mona.smoothing.smoothing.BaseFilter

There exist two types of filter implementations, which the user can choose from:

EKFConstVelocityConstYawrate: an Extended Kalman Filter with a nonlinear model with constant velocity and constant yaw rate
EKFPointMass: a Kalman Filter with a point mass model with acceleration in x and y direction as inputs to the model

Additional filter implementations can easily be added by deriving from BaseFilter.

For an overview to smoothing, check out Chapter 7 of (Särkkä, 2013):

Särkkä, S. (2013). Bayesian filtering and smoothing (No. 3). Cambridge University Press.

Alternative text — UML Diagram of the most important classes of the smoothing package.#

`EKFConstVelocityConstYawrate` class#

The state $x = [s_{x}, s_{y}, v, t h e t a, o m e g a]$ sits in the rear axle of the vehicle, where where s_x, s_y denote the x- and y- position of the vehicle, theta the heading, v the velocity, and omega the yaw rate.

The measurement is the center position $z = [x_{c}, y_{c}]$ . The resulting measurement function is $h = [x_{c} + L * c o s (t h e t a), y_{c} + L * s i n (t h e t a)]$ , which maps the state to the corresponding measurement. Furthermore, noise in the measurement is captured by the additive zero-mean Gaussian white noises

class mona.smoothing.ekf_const_velocity_const_yawrate.EKFConstVelocityConstYawrate#

Extended Kalman Filter class for constant yaw rate model.

State = [x,y,v,psi,omega] and measurement = [x_center,y_center]

__init__(dt, cov_measuring, var_process_acc=0.01, var_process_omega=0.01, l_r=1.5, **kwargs)#

Initialize EKFConstVelocityConstYawrate class.

Parameters:

dt (float) – time increment of input data
sigma_pos_x – standard deviation of measurement x position
sigma_pos_y – standard deviation of measurement y position
tau_v – process noise variance for velocity
tau_psi – process noise variance for yaw angle
tau_omega – process noise variance for yaw rate
l_r (float) – distance from the vehicle’s center to its rear axis
cov_measuring (ndarray) –
var_process_acc (float) –
var_process_omega (float) –

init_x(pos_x, pos_y, theta, vel)#

Set initial state of filter.

Parameters:

pos_x (float) – initial x coordinate of center position
pos_y (float) – initial y coordinate of center position
theta (float) – initial orientation
vel (float) – initial velocity

Return type:

None

calculate_F_jacobian_at(x)#

Calculate Jacobian F = df/dx of discrete state transition function f.

x[k+1] = f(x[k])

Parameters:: x (ndarray) – state vector
Return type:: ndarray

calculate_H_jacobian_at(x)#

Return Jacobian dh/dx.

(measurement function) where h = [x_rear + l_r * cos(psi), y_rear + l_r * sin(psi)]

Parameters:: x (ndarray) – state vector
Return type:: ndarray

calculate_hx(x)#

Map state variable (rear axis) to corresponding measurement z (center).

Parameters:: x (ndarray) – state vector
Return type:: ndarray

calculate_H_jacobian_pseudo(x)#

Calculate the Jacobian of the measurement matrix for a pseudo measurement.

The pseudo measurement is defined as [velocity, yaw rate]

Parameters:: x (ndarray) – State vector
Returns:: Jacobian matrix with shape [2, 5]
Return type:: ndarray

calculate_hx_pseudo(x)#

Project state x into the pseudo measurement space.

Parameters:: x (ndarray) – State vector
Returns:: Projected state vector [velocity, yaw rate]
Return type:: ndarray

update_pseudo_measurement(v_pseudo=0.0, yaw_rate_pseudo=0.0)#

Perform a pseudo measurement update to attenuate yaw noise.

Parameters:

v_pseudo (float) – Velocity of the pseudo measurement
yaw_rate_pseudo (float) – Yaw rate of the pseudo measurement

Return type:

None

propagate_x(x)#

Predict the next state of X.

Parameters:: x (ndarray) – state vector
Return type:: ndarray

`EKFPointMass` class#

The state $x = [x_{c}, y_{c}, v_{x}, v_{y}]$ sits in the center of the vehicle, which is denoted by subscript c.

The measurement is the center position $z = [x_{c}, y_{c}]$ . The resulting measurement function $h = [x_{c}, y_{c}]$ , which maps the state to the corresponding measurement, is trivial.

class mona.smoothing.ekf_point_mass.EKFPointMass#

Extended Kalman Filter class for Point Mass Model.

state = [x,y,vx,vy] and measurement = [x_center,y_center]

__init__(dt, var_meas_pos_x=0.5, var_meas_pos_y=0.5, var_process_v=0.1, **kwargs)#

Initializes EKFPointMass class.

Parameters:

dt – time increment of input data
sigma_pos_x – standard deviation of measurement x position
sigma_pos_y – standard deviation of measurement y position
tau_vx – process noise variance for x velocity
tau_vy – process noise variance y velocity

init_x(pos_x, pos_y, theta, vel)#

Set’s initial state of filter.

Parameters:

pos_x (float) – initial x coordinate of center position
pos_y (float) – initial y coordinate of center position
theta (float) – initial orientation
vel (float) – initial velocity

Return type:

None

calculate_F_jacobian_at(x)#

Calculates Jacobian F = df/dx of discrete state transition function f.

x[k+1] = f(x[k])

Parameters:: x (ndarray) – state vector
Return type:: ndarray

calculate_H_jacobian_at(x)#

Returns Jacobian dh/dx of the h matrix.

measurement function h = [x, y].

Parameters:: x (ndarray) – state vector
Return type:: ndarray

calculate_hx(x)#

Maps state variable to corresponding measurement z.

Parameters:: x (ndarray) – state vector
Return type:: ndarray

propagate_x(x)#

Predicts the next state of X.

Parameters:: x (ndarray) – state vector
Return type:: ndarray

Smoothing

Contents

Smoothing#

Trajectory class#

BaseSmoother class#

ModelBasedRtsSmoother class#

BaseFilter class#

EKFConstVelocityConstYawrate class#

EKFPointMass class#

`BaseSmoother` class#

`ModelBasedRtsSmoother` class#

`BaseFilter` class#

`EKFConstVelocityConstYawrate` class#

`EKFPointMass` class#