#include <hc_cc_state_space.hpp>

Public Member Functions | |
| virtual std::vector< Control > | get_controls (const State &state1, const State &state2) const =0 |
| Virtual function that returns controls of the shortest path from state1 to state2. More... | |
| std::vector< State > | get_path (const State &state1, const State &state2) const |
| Returns path from state1 to state2. More... | |
| std::vector< State_With_Covariance > | get_path_with_covariance (const State_With_Covariance &state1, const State &state2) const |
| Returns path including covariances from state1 to state2. More... | |
| HC_CC_State_Space (double kappa, double sigma, double discretization) | |
| Constructor. More... | |
| std::vector< State > | integrate (const State &state, const std::vector< Control > &controls) const |
| Returns integrated states given a start state and controls. More... | |
| State | integrate_ODE (const State &state, const Control &control, double integration_step) const |
| Returns integrated state given a start state, a control, and an integration step. More... | |
| std::vector< State_With_Covariance > | integrate_with_covariance (const State_With_Covariance &state, const std::vector< Control > &controls) const |
| Returns integrated states including covariance given a start state and controls. More... | |
| State | interpolate (const State &state, const std::vector< Control > &controls, double t) const |
| Returns interpolated state at distance t in [0,1] (percentage of total path length) More... | |
| void | set_filter_parameters (const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &controller) |
| Sets the parameters required by the filter. More... | |
Protected Attributes | |
| double | discretization_ |
| Discretization of path. More... | |
| EKF | ekf_ |
| Extended Kalman Filter for uncertainty propagation. More... | |
| HC_CC_Circle_Param | hc_cc_circle_param_ |
| Parameters of a hc-/cc-circle. More... | |
| double | kappa_ |
| Curvature, sharpness of clothoid. More... | |
| double | sigma_ |
Definition at line 45 of file hc_cc_state_space.hpp.
| steering::HC_CC_State_Space::HC_CC_State_Space | ( | double | kappa, |
| double | sigma, | ||
| double | discretization | ||
| ) |
Constructor.
Definition at line 28 of file hc_cc_state_space.cpp.
|
pure virtual |
Virtual function that returns controls of the shortest path from state1 to state2.
Implemented in steering::CC00_Reeds_Shepp_State_Space, steering::HC0pm_Reeds_Shepp_State_Space, steering::HCpm0_Reeds_Shepp_State_Space, steering::HCpmpm_Reeds_Shepp_State_Space, steering::HC00_Reeds_Shepp_State_Space, steering::CC00_Dubins_State_Space, steering::CCpmpm_Dubins_State_Space, steering::CC0pm_Dubins_State_Space, steering::CCpm0_Dubins_State_Space, steering::CC_Dubins_State_Space, steering::HC_Reeds_Shepp_State_Space, and Test_HC_CC_State_Space.
| vector< State > steering::HC_CC_State_Space::get_path | ( | const State & | state1, |
| const State & | state2 | ||
| ) | const |
Returns path from state1 to state2.
Definition at line 68 of file hc_cc_state_space.cpp.
| vector< State_With_Covariance > steering::HC_CC_State_Space::get_path_with_covariance | ( | const State_With_Covariance & | state1, |
| const State & | state2 | ||
| ) | const |
Returns path including covariances from state1 to state2.
Definition at line 74 of file hc_cc_state_space.cpp.
| vector< State > steering::HC_CC_State_Space::integrate | ( | const State & | state, |
| const std::vector< Control > & | controls | ||
| ) | const |
Returns integrated states given a start state and controls.
Definition at line 81 of file hc_cc_state_space.cpp.
|
inline |
Returns integrated state given a start state, a control, and an integration step.
Definition at line 281 of file hc_cc_state_space.cpp.
| vector< State_With_Covariance > steering::HC_CC_State_Space::integrate_with_covariance | ( | const State_With_Covariance & | state, |
| const std::vector< Control > & | controls | ||
| ) | const |
Returns integrated states including covariance given a start state and controls.
Definition at line 137 of file hc_cc_state_space.cpp.
| State steering::HC_CC_State_Space::interpolate | ( | const State & | state, |
| const std::vector< Control > & | controls, | ||
| double | t | ||
| ) | const |
Returns interpolated state at distance t in [0,1] (percentage of total path length)
Definition at line 212 of file hc_cc_state_space.cpp.
| void steering::HC_CC_State_Space::set_filter_parameters | ( | const Motion_Noise & | motion_noise, |
| const Measurement_Noise & | measurement_noise, | ||
| const Controller & | controller | ||
| ) |
Sets the parameters required by the filter.
Definition at line 62 of file hc_cc_state_space.cpp.
|
protected |
Discretization of path.
Definition at line 98 of file hc_cc_state_space.hpp.
|
protected |
Extended Kalman Filter for uncertainty propagation.
Definition at line 104 of file hc_cc_state_space.hpp.
|
protected |
Parameters of a hc-/cc-circle.
Definition at line 101 of file hc_cc_state_space.hpp.
|
protected |
Curvature, sharpness of clothoid.
Definition at line 95 of file hc_cc_state_space.hpp.
|
protected |
Definition at line 95 of file hc_cc_state_space.hpp.