├── .gitignore ├── LICENSE ├── README.md ├── VehicleParameters ├── SUVBicyleModelParameters.m ├── SedanBicycleModelParameters.m ├── SmallSUVBicycleModelParameters.m ├── SmallSedanBicycleModelParameters.m └── VehicleParametersBicycleModel.m ├── filter_classes ├── XKalmanFilter.m └── XKalmanPredictor.m ├── initialization.m ├── mmae_filters ├── AutonomousMultiModelFilter.m ├── ContextualBehaviorPredIMM.m ├── InteractiveMultiModelFilter.m └── SLContextualBehaviorPredIMM.m ├── motionModelClasses ├── ConstantAccelerationMotionModel.m ├── ConstantAccelerationZeroLateralVelMotionModel.m ├── ConstantLongVelocityStatLateralMotionModel.m ├── ConstantVelocityMotionModel.m ├── KinematicBicycleModel.m ├── LeftLaneChangeRelXYMotionModel.m ├── LeftLaneChangeRelativeMotionModel.m ├── LeftLaneChangeRelativeMotionModelWithAcc.m ├── LeftLaneChangeSinusoidalMotionModel.m ├── LinearizedBicycleModel.m ├── MeasurementModel.m ├── MildAccelerationZeroLateralVelMotionModel.m ├── MotionModel.m ├── NonLinearMotionModel.m ├── RightLaneChangeRelativeMotionModel.m ├── RightLaneChangeSinusoidalMotionModel.m ├── SimplifiedKinematicBicycleModel.m ├── ZeroAccelerationAndLateralVelMotionModel.m └── ZeroAccelerationMotionModel.m ├── readme_images ├── aggressive_driver_scenario.png └── passive_driver_scenario.png └── sim_scenarios ├── contextual_IMM_main.m ├── ground_truth_and_measurements.m ├── lane_change_noisy_measurements_generator.m ├── post_processing_plots.m └── pred_post_processing_matlab_ws_x_coord_mod.m /.gitignore: -------------------------------------------------------------------------------- 1 | vehicle_dynamics/ 2 | *.m~ 3 | slprj/ 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Interaction-Aware Probabilistic Framework for Autonomous Vehicles to Predict Behaviors and Driving Policy of Traffic Participants 2 | 3 | This work provides a proof-of-concept for an interaction-aware behavior prediction framework for autonomous vehicles that not only predicts the behaviors of the traffic participants but also classifies the driving policy that can be associated with them. The work was developed as a part of following PhD Dissertation. 4 | 5 | **Title**: Probabilistic Framework for Behavior Characterization of Traffic Participants Enabling Long Term Prediction 6 | 7 | **Abstract**: This research aims at developing new methods that predict the behaviors of the human driven traffic participants, enabling safe operation of autonomous vehicles in complex traffic environments. Autonomous vehicles are expected to operate amongst human driven conventional vehicles in the traffic at least for the next few decades. For safe navigation they will need to infer the intents as well as the behaviors of the human traffic participants using extrinsically observable information. Doing so aides in predicting their trajectories for a time horizon long enough to analyze imminent risks and gracefully avert any risky situation. This work approaches above challenge by recognizing that any maneuver performed by a human driver can be divided into four stages that depend on the surrounding context: intent determination, maneuver preparation, gap acceptance and maneuver execution. It builds on the hypothesis that for a given driver, the behavior not only spans across these four maneuver stages, but across multiple maneuvers. As a result, identifying the driver behavior in any of these stages can help characterize the nature of all the subsequent maneuvers that the driver is likely to perform, thus resulting in a more accurate prediction for a longer time horizon. To enable this, a novel probabilistic framework is realized that couples the different maneuver stages of the observed traffic participant together and associates them to a driving style. The framework is implemented by modeling a traffic participant as a hybrid system. The contextual information of the observed traffic participant is utilized in extending t he Interacting Multiple Model (IMM) and classification of the driving style of the traffic participant (behavior characterization) is demonstrated for two use case scenarios in this repository. The developed Contextual IMM (CIMM) framework shows improvements in the performance of the behavior classification of the traffic participants compared to the IMM for the identified use case scenarios. Further, it also estimates the driving style of a traffic participant. This outcome warrants strong potential of this approach for autonomous vehicles on public roads. 8 | 9 | For further details, please refer to the [PhD dissertation](https://tigerprints.clemson.edu/all_dissertations/2509) detailing this work. 10 | 11 | ## Citation: 12 | Gill, Jasprit Singh, "Probabilistic Framework for Behavior Characterization of Traffic Participants Enabling Long Term Prediction" (2019). All Dissertations. 2509. 13 | https://tigerprints.clemson.edu/all_dissertations/2509 14 | 15 | ## Framework demonstration 16 | The framework allows modeling different driver policies ( aggressive, passive, cooperative, non-cooperative, etc.) and classifies which policy the observed traffic participant is driving with. It also utilizes this information in predicting their future behaviors. To model driver policies, the framework provides hooks for: 17 | 18 | 1) the gap acceptance criteria for different driver policies 19 | 2) the maneuver selection criteria for different driver policies 20 | 21 | The provided source code demonstrates how to set up the framework with two different driver policies (aggressive and passive driver policy) and use it to run inference on a traffic participant. Click on the images below to see the animations generated from the matlab source. 22 | 23 | #### Aggressive Driver scenario: 24 | [![Aggressive Drive Scenario](https://github.com/jollysg/contextual_behavior_prediction/blob/master/readme_images/aggressive_driver_scenario.png)](https://www.youtube.com/embed/viispI7NazM) 25 | 26 | #### Passive Driver scenario: 27 | [![Passive Drive Scenario](https://github.com/jollysg/contextual_behavior_prediction/blob/master/readme_images/passive_driver_scenario.png)](https://www.youtube.com/embed/KN_oYHsRDd4) 28 | 29 | The simulated video shows 3 subplots: 30 | 1. Participant Trajectories: This subplot shows the trajectories followed by the 3 participants chosen for the 2 lane driving scenario. Y coordinate of 0 represents the center of the right lane and 3.5m represents the center of the left lane. The observed vehicle is in the right lane (bottom). Red line represents the trajectory tracked (i.e. current and past positions) by the observed vehicle and yellow represents the predicted trajectory for it at any time instant. The predictions take into account the probabilities of the behavior type as well as the driver types identified for it at any instant. Purple line represents the preceeding vehicle position in front of the observed vehicle in the right lane. Green line represents the position of the another traffic participant in the left lane. 31 | 32 | 2. Behavior Probabilities: This subplot shows the probabilistic measures of the behavior identified for the observed vehicle by the behavior prediction model after taking into account the vehicle measurements and the context around it. Four behaviors were considered for this scenario - a passive straight, an aggressive straight (with heavy acceleration), short aggressive lane change and long passive lane change. 33 | 34 | 3. Driver type weights vs time: This subplot shows the probabilitic measures of the driver type detected for the observed vehicle by the behavior prediction model after taking into account the vehicle measurements and the context around it. 35 | 36 | ## Source Code Instructions 37 | At present, the developed model is in MATLAB source files. The source for C++/Python is in works and will be available soon. 38 | 39 | 40 | 1. To ensure all the relevant files are included in the MATLAB path, run the initialization.m file from the root folder. 41 | 2. The entry point for the source is in sim_scenarios folder. 42 | - Run the 'lane_change_noisy_measurements_generator.m' file in the sim_scenarios folder. It generates the ground truth for the simulated scenarios and calls the contextual_IMM_main.m file. To aggressive or passive driver scenarios can be switched using the 'aggressive_driver_use_case' flag in this file. 43 | - contextual_IMM_main.m runs the predictions for the scenarios modeled and calls post_processing_plots.m. 44 | - post_processing_plots.m generates the plots for the prediction model. 45 | 3. Finally, run pred_post_processing_matlab_ws_x_coord_mod.m for animations of the scenario. 46 | 4. [SLContextualBehaviorPredIMM class](https://github.com/jollysg/contextual_behavior_prediction/blob/master/mmae_filters/SLContextualBehaviorPredIMM.m) demonstrates how to set up the scenario and motionmodels for behavior prediction. 47 | 5. To experiment with different motion models or customize to your own scenario, a good way to start is to make a copy of SLContextualBehaviorPredIMM class and modify it. 48 | 6. Some sample motion models are provided in the [motionModelClasses](https://github.com/jollysg/contextual_behavior_prediction/tree/master/motionModelClasses) folder. To experiment with your own motion models, simply inherit from either the [MotionModel](https://github.com/jollysg/contextual_behavior_prediction/blob/master/motionModelClasses/MotionModel.m) class or [NonLinearMotionModel](https://github.com/jollysg/contextual_behavior_prediction/blob/master/motionModelClasses/NonLinearMotionModel.m) class. 49 | 7. The code also provides sources for Linear as well as extended kalman filter ([XKalmanFilter](https://github.com/jollysg/contextual_behavior_prediction/tree/master/filter_classes)), Autonomous Multiple Model (AMM) filter and Interactive-Multiple Model filter (IMM) in the [mmae_filters](https://github.com/jollysg/contextual_behavior_prediction/tree/master/mmae_filters) folder. 50 | 51 | Queries: jaspritsgill@gmail.com 52 | 53 | To learn more about my work, please visit my website: https://www.jaspritsgill.com/ 54 | -------------------------------------------------------------------------------- /VehicleParameters/SUVBicyleModelParameters.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SUVBicyleModelParameters < VehicleParametersBicycleModel 24 | properties 25 | end 26 | 27 | methods 28 | function obj = SUVBicyleModelParameters() 29 | % need realistic values (C_tire too low for an SUV maybe?) 30 | obj.m = 2100; 31 | obj.l_f = 1.3; 32 | obj.l_r = 1.5; 33 | obj.Jz = 3900; 34 | obj.C_f = 2*80000; 35 | obj.C_r = 2*80000; 36 | end 37 | end 38 | end -------------------------------------------------------------------------------- /VehicleParameters/SedanBicycleModelParameters.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SedanBicycleModelParameters < VehicleParametersBicycleModel 24 | properties 25 | % inherits properties from VehicleParametersBicycleModel, any 26 | % additional come here 27 | end 28 | 29 | methods 30 | function obj = SedanBicycleModelParameters() 31 | obj.m = 1573; 32 | obj.l_f = 1.1; 33 | obj.l_r = 1.58; 34 | obj.Jz = 2873; 35 | c_tire_front = 80000; % N/rad per tire 36 | c_tire_rear = 80000; %N / rad per tire 37 | obj.C_f = c_tire_front*2; %160000 N/rad 38 | obj.C_r = c_tire_rear*2; %160000 N/rad 39 | end 40 | end 41 | 42 | end -------------------------------------------------------------------------------- /VehicleParameters/SmallSUVBicycleModelParameters.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SmallSUVBicycleModelParameters < VehicleParametersBicycleModel 24 | properties 25 | end 26 | 27 | methods 28 | function obj = SmallSUVBicycleModelParameters() 29 | obj.m = 1830; 30 | obj.l_f = 1.211; 31 | obj.l_r = 1.459; 32 | obj.Jz = 3070; 33 | c_tire = 1830; % kg/deg per tire 34 | multiplier = 0.165; % 16-17% of tire load per 1 rad of slip angle 35 | distribution = 0.5; % equal between front and rear 36 | gravity = 9.81; 37 | obj.C_f = 2 * c_tire * distribution * multiplier * 180/pi * gravity; %84859 N/rad/tire = 169720 38 | obj.C_r = obj.C_f; 39 | end 40 | end 41 | end -------------------------------------------------------------------------------- /VehicleParameters/SmallSedanBicycleModelParameters.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SmallSedanBicycleModelParameters < VehicleParametersBicycleModel 24 | properties 25 | end 26 | 27 | methods 28 | function obj = SmallSedanBicycleModelParameters() 29 | obj.m = 1150; 30 | obj.l_f = 1.084; 31 | obj.l_r = 1.596; 32 | obj.Jz = 1850; 33 | c_axle_front = 1270.6; % N/deg per axle 34 | c_axle_rear = 949.2; %N / deg per axle 35 | obj.C_f = c_axle_front * 180/pi; %72766 N/rad 36 | obj.C_r = c_axle_rear * 180/pi; %54395 N/rad 37 | end 38 | end 39 | 40 | end -------------------------------------------------------------------------------- /VehicleParameters/VehicleParametersBicycleModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef VehicleParametersBicycleModel 24 | %default vehicle parameters 25 | properties 26 | %mass (Kgs) 27 | m 28 | %dist of front axle from CG (m) 29 | l_f 30 | %dis of rear axle from CG (m) 31 | l_r 32 | %Moment of Inertia (Kg-m2) 33 | Jz 34 | %Cornering stiffness front axle (Kg/rad) (~ 2 times the tires) 35 | C_f 36 | %cornering stiffness rear axle (Kg/rad) (~ 2 times the tires) 37 | C_r 38 | end 39 | 40 | methods 41 | end 42 | end -------------------------------------------------------------------------------- /filter_classes/XKalmanFilter.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef XKalmanFilter < handle %& matlab.mixin.Copyable 24 | properties 25 | Ts 26 | predicted_state 27 | predicted_P 28 | 29 | estimatedState 30 | estimate_P 31 | % Measurement noise 32 | R 33 | % Process noise 34 | Q 35 | % Motion model 36 | mm 37 | % likelihood 38 | likelihood 39 | % error innovation 40 | err_innov 41 | % error covariance 42 | err_cov 43 | % experimental 44 | weight 45 | nonNormalizedWeight 46 | no_of_states 47 | end 48 | 49 | methods 50 | function self = XKalmanFilter(Ts, motionmodel) 51 | if nargin == 0 52 | Ts = 0.1; 53 | motionmodel = LinearizedBicycleModel(); 54 | elseif nargin == 1 55 | motionmodel = LinearizedBicycleModel(Ts); 56 | end 57 | 58 | self.Ts = Ts; 59 | self.mm = motionmodel; 60 | self.weight = 1; 61 | self.nonNormalizedWeight = 1; 62 | self.estimatedState = motionmodel.states; 63 | self.estimate_P = eye(length(self.estimatedState)); 64 | self.predicted_state = motionmodel.states; 65 | self.predicted_P = eye(length(self.estimatedState)); 66 | self.likelihood = 0; 67 | self.Q = self.estimate_P*0.001; 68 | self.err_innov = motionmodel.output_states; 69 | self.R = eye(length(self.err_innov))*0.0025; 70 | self.err_cov = self.R; 71 | self.no_of_states = length(self.estimatedState); 72 | end 73 | 74 | function self = updateNoiseStatistics(self, Q, R) 75 | self.Q = Q; 76 | self.R = R; 77 | end 78 | 79 | function setInitialConditions(self, X, P) 80 | self.estimatedState = X; 81 | self.estimate_P = P; 82 | self.predicted_state = X; 83 | self.predicted_P = P; 84 | self.mm.reset(X); 85 | end 86 | 87 | function [x_prop, p_prop] = predict(self, u, x, P) 88 | if nargin == 2 89 | % Use the last propagated states for propagating further 90 | x = self.predicted_state; 91 | P = self.predicted_P; 92 | end 93 | x_prop = self.mm.propagate(x, u); 94 | 95 | % The first term in the covariance equation can be moved into 96 | % motion model for abstraction, but it will make the motion 97 | % model closely coupled with KF 98 | F = self.mm.linearizedDiscreteStateTransitionMatrix(x, u); 99 | p_prop = (F * P * F') + self.Ts*self.Q; 100 | 101 | %also predict for the next few time steps 102 | 103 | self.predicted_state = x_prop; 104 | self.predicted_P = p_prop; 105 | end 106 | 107 | function [x_plus, p_plus] = correct(self, y_tilde, x_prop, P_prop) 108 | if nargin == 2 109 | x_prop = self.predicted_state; 110 | P_prop = self.predicted_P; 111 | end 112 | 113 | %correct 114 | yhat_minus = self.mm.estimatedMeasurement(x_prop, 0); 115 | self.err_innov = y_tilde - yhat_minus; %residual 116 | 117 | % Following and the first term of error cov can be moved into 118 | % motion model, but it make the motion model coupled closely 119 | % with kalman filter and we wan't to keep that indepent. 120 | Ck = self.mm.getOutputMatrix(); 121 | self.err_cov = Ck * P_prop *Ck' + self.R; %residual covariance 122 | K = P_prop * Ck' * inv(self.err_cov); 123 | x_plus = x_prop + K * self.err_innov; 124 | p_plus = (eye(length(x_prop)) - K * Ck) * P_prop; 125 | self.estimatedState = x_plus; 126 | self.mm.setStates(x_plus); 127 | self.estimate_P = p_plus; 128 | 129 | % after correction, predicted and estimated states should be 130 | % the same. 131 | self.predicted_state = x_plus; 132 | self.predicted_P = p_plus; 133 | 134 | self.likelihood = 1/sqrt(det(2*pi*self.err_cov)) ... 135 | * exp(-1/2 * self.err_innov'* inv(self.err_cov) * self.err_innov); 136 | 137 | % self.nonNormalizedWeight = self.weight * self.likelihood; 138 | 139 | end 140 | end 141 | end -------------------------------------------------------------------------------- /filter_classes/XKalmanPredictor.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef XKalmanPredictor < XKalmanFilter 24 | properties 25 | predictions_state 26 | predictions_P 27 | end 28 | 29 | methods 30 | function self = XKalmanPredictor(Ts, motionmodel) 31 | self@XKalmanFilter(Ts, motionmodel); 32 | no_of_states = length(self.estimatedState); 33 | fsteps = 5/self.Ts; 34 | self.predictions_state = zeros([size(self.estimatedState), fsteps]); 35 | self.predictions_P = zeros([size(self.estimate_P), fsteps]); 36 | 37 | end 38 | 39 | function [x_prop, p_prop] = predict(self, u, x, P) 40 | if nargin == 2 41 | % Use the last propagated states for propagating further 42 | x = self.estimatedState; 43 | P = self.estimate_P; 44 | end 45 | 46 | no_of_states = length(self.estimatedState); 47 | 48 | % also predict for the next few time steps (4-5 secs) 49 | fsteps = 5/self.Ts; 50 | pred_X = zeros([size(x), fsteps]); 51 | pred_P = zeros([size(P), fsteps]); 52 | 53 | for i = 1:fsteps 54 | x = self.mm.propagate(x, u); 55 | 56 | % The first term in the covariance equation can be moved 57 | % into motion model for abstraction, but it will make the 58 | % motion model closely coupled with KF 59 | F = self.mm.linearizedDiscreteStateTransitionMatrix(x, u); 60 | P = (F * P * F') + self.Ts*self.Q; 61 | pred_X(1:no_of_states,1, i) = x; 62 | pred_P(1:no_of_states,1:no_of_states, i) = P; 63 | end 64 | 65 | self.predicted_state = pred_X(1:no_of_states,1, 1); 66 | self.predicted_P = pred_P(1:no_of_states, 1:no_of_states, 1); 67 | self.predictions_state = pred_X; 68 | self.predictions_P = pred_P; 69 | x_prop = self.predicted_state; 70 | p_prop = self.predicted_P; 71 | end 72 | 73 | function [x_plus, p_plus] = correct(self, y_tilde, x_prop, P_prop) 74 | if nargin == 2 75 | x_prop = self.predicted_state; 76 | P_prop = self.predicted_P; 77 | end 78 | 79 | %correct 80 | yhat_minus = self.mm.estimatedMeasurement(x_prop, 0); 81 | self.err_innov = y_tilde - yhat_minus; %residual 82 | 83 | % Following and the first term of error cov can be moved into 84 | % motion model, but it make the motion model coupled closely 85 | % with kalman filter and we wan't to keep that indepent. 86 | Ck = self.mm.getOutputMatrix(); 87 | self.err_cov = Ck * P_prop *Ck' + self.R; %residual covariance 88 | K = P_prop * Ck' * inv(self.err_cov); 89 | x_plus = x_prop + K * self.err_innov; 90 | p_plus = (eye(length(x_prop)) - K * Ck) * P_prop; 91 | self.estimatedState = x_plus; 92 | self.mm.setStates(x_plus); 93 | self.estimate_P = p_plus; 94 | 95 | % after correction, predicted and estimated states should be 96 | % the same. 97 | self.predicted_state = x_plus; 98 | self.predicted_P = p_plus; 99 | 100 | self.likelihood = 1/sqrt(det(2*pi*self.err_cov)) ... 101 | * exp(-1/2 * self.err_innov'* inv(self.err_cov) * self.err_innov); 102 | % self.nonNormalizedWeight = self.weight * self.likelihood; 103 | 104 | end 105 | end 106 | end -------------------------------------------------------------------------------- /initialization.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | clear; 24 | 25 | % Get path of this file, and add the parent folder and the subfolders into 26 | % PATH 27 | % which('') returns the full path of the file 28 | % mfilename('fullpath') returns the full path of the file. Without any 29 | % arguments, this function returns just the filename 30 | % pwd returns the current working directory of matlab, which can be 31 | % different from the current file directory 32 | fullFilename = mfilename('fullpath') 33 | % get the parent folder from the full filepath 34 | [filepath, name, ext] = fileparts(fullFilename); 35 | 36 | % addpath('./motionModelClasses') 37 | disp('Following files will be added to the path...'); 38 | for folder = ["/motionModelClasses", "/mmae_filters", "/filter_classes", "/VehicleParameters"] 39 | folder_path = append(filepath, folder); 40 | disp(folder_path) 41 | addpath(folder_path) 42 | end 43 | -------------------------------------------------------------------------------- /mmae_filters/AutonomousMultiModelFilter.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef AutonomousMultiModelFilter < handle 24 | properties 25 | Ts 26 | elementalFilters 27 | % elementalFilterLikelihoods 28 | % estimates_elementalFilters 29 | % P_estimates_elementalFilters 30 | weights 31 | combined_estimate 32 | P_combined_estimate 33 | no_of_states 34 | end 35 | 36 | methods 37 | function self = AutonomousMultiModelFilter(Ts) 38 | if nargin == 0 39 | Ts = 0.1; 40 | end 41 | self.Ts = Ts; 42 | self.no_of_states = 4; 43 | % By default, initializes everything to double empty, elemental 44 | % filters need to be initialized to XKalmanFilter empty array. 45 | % Unfortunately it is not supported for code generation. So we 46 | % need to use a cell array. However, dot indexing is not 47 | % supported when using a for loop. So its best to index it with 48 | % an iterator (like i) and then set the elements. Second, the 49 | % compiler gives an error if the array of objects are 50 | % initialized dynamically. So the best 51 | % self.elementalFilters = {}; 52 | self.combined_estimate = zeros(self.no_of_states, 1); 53 | self.P_combined_estimate = eye(self.no_of_states); 54 | end 55 | 56 | function addElementalFilter(self, elementalFilter) 57 | num_filters = length(self.elementalFilters) + 1; 58 | self.elementalFilters{num_filters} = elementalFilter; 59 | self.resetFilterWeights(); 60 | end 61 | 62 | function resetFilterWeights(self) 63 | num_filters = length(self.elementalFilters); 64 | for i = 1:num_filters 65 | flt = self.elementalFilters{i}; 66 | flt.weight = 1/num_filters; 67 | end 68 | end 69 | 70 | function predict(self, u, X, P) 71 | if nargin == 2 72 | X = self.combined_estimate; 73 | P = self.P_combined_estimate; 74 | end 75 | 76 | for i = 1: length(self.elementalFilters) 77 | flt = self.elementalFilters{i}; 78 | flt.predict(u); 79 | end 80 | end 81 | 82 | function correct(self, y_tilde, X, P) 83 | if nargin == 2 84 | X = self.combined_estimate; 85 | P = self.P_combined_estimate; 86 | end 87 | 88 | for i = 1: length(self.elementalFilters) 89 | flt = self.elementalFilters{i}; 90 | flt.correct(y_tilde); 91 | end 92 | self.updateWeights(); 93 | end 94 | 95 | function updateWeights(self) 96 | normalizer = 0; 97 | for i = 1: length(self.elementalFilters) 98 | flt = self.elementalFilters{i}; 99 | flt.nonNormalizedWeight = flt.weight * flt.likelihood; 100 | normalizer = normalizer + flt.nonNormalizedWeight; 101 | end 102 | 103 | for i = 1: length(self.elementalFilters) 104 | flt = self.elementalFilters{i}; 105 | flt.weight = flt.nonNormalizedWeight/normalizer; 106 | end 107 | end 108 | 109 | function wts = getFilterWeights(self) 110 | num_filters = length(self.elementalFilters); 111 | wts = zeros(1, num_filters); 112 | for i = 1:num_filters 113 | flt = self.elementalFilters{i}; 114 | wts(i) = flt.weight; 115 | end 116 | end 117 | 118 | function lhd = getFilterLikelihoods(self) 119 | num_filters = length(self.elementalFilters); 120 | lhd = zeros(1, num_filters); 121 | for i = 1:num_filters 122 | flt = self.elementalFilters{i}; 123 | lhd(i) = flt.likelihood; 124 | end 125 | end 126 | 127 | function wts = getFilterNonNormalizedWeights(self) 128 | num_filters = length(self.elementalFilters); 129 | wts = zeros(1, num_filters); 130 | for i = 1:num_filters 131 | flt = self.elementalFilters{i}; 132 | wts(i) = flt.nonNormalizedWeight; 133 | end 134 | end 135 | 136 | function err = getFilterErrors(self) 137 | num_filters = length(self.elementalFilters); 138 | flt = self.elementalFilters{1}; 139 | no_of_observ = length(flt.mm.output_states); 140 | err = zeros(no_of_observ, 1, num_filters); 141 | for i = 1:num_filters 142 | flt = self.elementalFilters{i}; 143 | err(:,:, i) = flt.err_innov; 144 | end 145 | end 146 | 147 | 148 | % functions that will have to be overridden in the super class due 149 | % to length of the states. 150 | 151 | function setInitialConditions(self, X, P) 152 | for i = 1:length(self.elementalFilters) 153 | flt = self.elementalFilters{i}; 154 | flt.setInitialConditions(X, P); 155 | end 156 | end 157 | 158 | function [comb_est, comb_P] = calculateCombinedEstimate(self) 159 | comb_est = [0 0 0 0]'; 160 | for i = 1: length(self.elementalFilters) 161 | flt = self.elementalFilters{i}; 162 | estimate = flt.estimatedState(1:self.no_of_states); 163 | comb_est = comb_est + flt.weight * estimate; 164 | end 165 | self.combined_estimate = comb_est; 166 | 167 | % calculate combined covariance 168 | 169 | comb_P = zeros(4); 170 | for i = 1: length(self.elementalFilters) 171 | flt = self.elementalFilters{i}; 172 | estimate = flt.estimatedState(1:self.no_of_states); 173 | est_P = flt.estimate_P(1:self.no_of_states, 1:self.no_of_states); 174 | err = estimate - comb_est; 175 | comb_P = comb_P + flt.weight * (err * err' + est_P); 176 | end 177 | self.P_combined_estimate = comb_P; 178 | end 179 | 180 | function est_cov = getFilterEstimateCovariances(self) 181 | num_filters = length(self.elementalFilters); 182 | est_cov = zeros(self.no_of_states, self.no_of_states, num_filters); 183 | for i = 1:num_filters 184 | flt = self.elementalFilters{i}; 185 | est_cov(:,:, i) = flt.predicted_P(1:self.no_of_states, 1:self.no_of_states); 186 | end 187 | end 188 | 189 | function est = getFilterEstimates(self) 190 | num_filters = length(self.elementalFilters); 191 | est = zeros(4, 1, num_filters); 192 | for i = 1:num_filters 193 | flt = self.elementalFilters{i}; 194 | est(:,:, i) = flt.predicted_state(1:self.no_of_states); 195 | end 196 | end 197 | 198 | end 199 | end -------------------------------------------------------------------------------- /mmae_filters/ContextualBehaviorPredIMM.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ContextualBehaviorPredIMM < HighwayAllBehaviorsIMM 24 | properties 25 | % context vector = 6 distances around 26 | context 27 | % aggressive and passive driver probabilities [P(d1); P(d2)]; 28 | driverTypes 29 | % driver thresholds for gaps (distances for now) 30 | driverThresholds 31 | % gap acceptance probabilities [P(G = 0); P(G = 1)] 32 | gapAcceptance 33 | end 34 | methods 35 | function self = ContextualBehaviorPredIMM(Ts) 36 | self@HighwayAllBehaviorsIMM(Ts); 37 | self.context = zeros(6,1); 38 | % start with 2 drivers - aggressive and passive by default 39 | self.driverTypes = [0.5, 0.5]; 40 | self.thresholds = zeros(2, 1); 41 | self.gapAcceptance = [1; 0]; 42 | end 43 | 44 | function extractContext(self, participantStates) 45 | % This is the helper function for extracting the context. 46 | % PArticipant states should be the states of all the traffic 47 | % participants. The context vector needs to be extracted from 48 | % it. For now this can simply be context 49 | % context vector = 6 distances around 50 | self.context = participantStates; 51 | end 52 | 53 | function gap = gapAcceptanceDriverPolicy(self, threshold) 54 | c = self.context; 55 | %gap = [ P(G=0; P(G=1)] 56 | laneSatisfaction = (c(3) + c(4)) > threshold; % perhaps c(4) is not needed here 57 | leftLaneDissatisfaction = (c(1) + c(2)) <= threshold; 58 | rightLaneDissatisfaction = (c(5) + c(6)) <= threshold; 59 | gap_rejected = double(laneSatisfaction | (leftLaneDissatisfaction & rightLaneDissatisfaction)); 60 | gap = [gap_rejected; 1 - gap_rejected]; 61 | end 62 | 63 | function gapAcceptancePolicy(self) 64 | % aggressive 65 | d1 = self.driverTypes(1); 66 | % passive 67 | d2 = self.driverTypes(2); 68 | gapAcceptAgg = self.gapAcceptanceDriverPolicy(self.driverThresholds(1)); 69 | gapAcceptPass = self.gapAcceptanceDriverPolicy(self.driverThresholds(2)); 70 | self.gapAcceptance = d1 * gapAcceptAgg + d2 * gapAcceptPass; 71 | normalizer = sum(self.gapAcceptance); 72 | self.gapAcceptance = self.gapAcceptance/normalizer; 73 | end 74 | 75 | function prob = leftLaneChangeManeuverFunction(self) 76 | c = self.context; 77 | num = c(1) + c(2); 78 | den = num + c(5) + c(6); 79 | % return c1 + c2 / (c1 + c2 + c5 + c6); 80 | prob = num/den; 81 | end 82 | 83 | function prob = rightLaneChangeManeuverFunction(self) 84 | c = self.context; 85 | num = c(5) + c(6); 86 | den = num + c(1) + c(2); 87 | % return c5 + c6 / (c1 + c2 + c5 + c6); 88 | prob = num/den; 89 | end 90 | 91 | function calculateBehaviorProbabilityTransitionMatrix(self) 92 | d1 = self.driverTypes(1); 93 | d2 = self.driverTypes(2); 94 | g_na = self.gapAcceptance(1); 95 | g_a = self.gapAcceptance(2); 96 | 97 | if (g_na < 0.1) 98 | % If g_na is very small, then add 0.01 to it, as there is 1 99 | % in 100 chance (assuming about 10 secs of maneuver length) 100 | % that the maneuver will be complete and the participant 101 | % will switch to default maneuver. If g_na is bigger than 102 | % 0.1, then the influence of this chance can be neglected. 103 | % Of course, if g_na is adjusted by 0.01, g_a will have to 104 | % adjusted too 105 | g_na = g_na + 0.01; 106 | g_a = 1 - g_na; 107 | end 108 | 109 | beh_prob_trans_matrix = [g_a 0 0 0 g_na; 110 | 0 g_a 0 0 g_na; 111 | 0 0 g_a 0 g_na; 112 | 0 0 0 g_a g_na; 113 | d1*ga*fll d2*ga*fll d1*ga*frl d2*ga*frl g_na]; 114 | 115 | % Normalize the matrix 116 | normalizers = sum(beh_prob_trans_matrix'); 117 | normalized_ptm = beh_prob_trans_matrix ./ normalizers'; 118 | 119 | % Now can it be mapped directly to transition matrix? 120 | self.markov_transition_matrix = normalized_ptm; 121 | end 122 | 123 | function driverUpdate(self) 124 | % this should be called after updating the probabilistic 125 | % weights of all the behaviors (i.e. elemental filters) 126 | w = self.weights; 127 | d1 = w(1) + w(3) + self.driverTypes(1)*w(5); 128 | d2 = w(2) + w(4) + self.driverTypes(2)*w(5); 129 | updated_driver_weights = [d1;d2]; 130 | normalizer = d1 + d2; 131 | self.driverTypes = updated_driver_weights/normalizer; 132 | end 133 | end 134 | end -------------------------------------------------------------------------------- /mmae_filters/InteractiveMultiModelFilter.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef InteractiveMultiModelFilter < AutonomousMultiModelFilter 24 | properties 25 | mixed_init_state 26 | mixed_init_state_cov 27 | markov_transition_matrix 28 | normalizers 29 | end 30 | 31 | methods 32 | function self = InteractiveMultiModelFilter(Ts) 33 | if nargin == 0 34 | Ts = 0.1; 35 | end 36 | self.Ts = Ts; 37 | self.markov_transition_matrix = eye(3); 38 | % self.elementalFilters = {}; 39 | end 40 | 41 | function updateMarkovTransitionMatrix(self, Pij) 42 | self.markov_transition_matrix = Pij; 43 | end 44 | 45 | function predict(self, u, X, P) 46 | if nargin == 2 47 | % TODO: Figure out what to do here, following is not 48 | % needed, but is only to preallocate the X and P matrices. 49 | % There are better ways to do this. 50 | X = self.mixed_init_state(:,:,1); 51 | P = self.mixed_init_state_cov(:,:,1); 52 | end 53 | % mixing happens here 54 | for i = 1: length(self.elementalFilters) 55 | flt = self.elementalFilters{i}; 56 | X = flt.predicted_state; 57 | P = flt.predicted_P; 58 | X(1:self.no_of_states) = self.mixed_init_state(:,:,i); 59 | P(1:self.no_of_states, 1:self.no_of_states) = self.mixed_init_state_cov(:,:,i); 60 | flt.predict(u, X, P); 61 | end 62 | end 63 | 64 | function updateWeights(self) 65 | normalizer = 0; 66 | for i = 1: length(self.elementalFilters) 67 | flt = self.elementalFilters{i}; 68 | flt.nonNormalizedWeight = self.normalizers(i)* flt.likelihood; 69 | normalizer = normalizer + flt.nonNormalizedWeight; 70 | end 71 | 72 | for i = 1: length(self.elementalFilters) 73 | flt = self.elementalFilters{i}; 74 | flt.weight = flt.nonNormalizedWeight/normalizer; 75 | end 76 | end 77 | 78 | function mixEstimates(self) 79 | wts = self.getFilterWeights(); 80 | no_wts = length(wts); 81 | mix_wts_matrix = zeros(no_wts); 82 | for i = 1:no_wts 83 | mix_wts_matrix(:,i) = wts' .* self.markov_transition_matrix(:,i); 84 | end 85 | % normalizers are basically the predicted probability of the 86 | % the system being in the mode i (i.e. filter i being the one 87 | % with higher likelihood). This is nothing but the weights of 88 | % all the filters in the last cycle multiplied by the markov 89 | % mode transition probabilities for transition to mode i. Since 90 | % this is the beginning of the current cycle, and usually 91 | % before the measurements have arrived, these are called as 92 | % predicted probabilities. 93 | normalizers = sum(mix_wts_matrix); 94 | 95 | for i = 1:no_wts 96 | for j = 1:length(normalizers) 97 | if normalizers(j) > 1e-20 98 | mix_wts_matrix(i,j) = mix_wts_matrix(i,j) ./ normalizers(j); 99 | else 100 | normalizers(j) = 0; 101 | mix_wts_matrix(i,j) = 0; 102 | end 103 | end 104 | end 105 | 106 | self.normalizers = normalizers'; 107 | % mix wts matrix shows the conditional probabilities wij that 108 | % the filter will be in the state j, if it were in state i in 109 | % the last cycle. 110 | %mix_Wts_matrix = [ w11 w12 w13; 111 | % w21 w22 w23; 112 | % w31 w32 w33]; 113 | 114 | % eg xo_(j) = w_1j * x1 + w_2j * x2 + w_3j * x3; 115 | 116 | % number of init_states will be for number of filters, which is 117 | % the same as the number of weights we found above 118 | self.mixed_init_state = zeros(self.no_of_states, 1, no_wts); 119 | filterEstimates = self.getFilterEstimates(); 120 | 121 | % mixed initial states 122 | for j = 1:no_wts 123 | mixed_state = zeros(self.no_of_states, 1); 124 | for i = 1:no_wts 125 | % mixed_state = mixed_State + w_ij * xi 126 | mixed_state = mixed_state + mix_wts_matrix(i,j) * filterEstimates(:,:,i); 127 | end 128 | self.mixed_init_state(:,:,j) = mixed_state; 129 | end 130 | 131 | % mixed initial state covariances 132 | self.mixed_init_state_cov = zeros(self.no_of_states, self.no_of_states, no_wts); 133 | covariances = self.getFilterEstimateCovariances(); 134 | 135 | % mix initial state covariances 136 | for j = 1: no_wts 137 | % for every filter, calculate the covaraince influence due 138 | % to every other filter 139 | 140 | % init zeros for covariance first 141 | mixed_cov = zeros(self.no_of_states); 142 | for i = 1: no_wts 143 | % iterate through the wts of every filter 144 | 145 | %relative error from the mixed_init_mean 146 | err = filterEstimates(:,:,i) - self.mixed_init_state(:,:,j); 147 | mixed_cov = mixed_cov + mix_wts_matrix(i,j) ... 148 | * (covariances(:,:,i) + err * err'); 149 | end 150 | self.mixed_init_state_cov(:,:,j) = mixed_cov; 151 | end 152 | end 153 | 154 | end 155 | end -------------------------------------------------------------------------------- /mmae_filters/SLContextualBehaviorPredIMM.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SLContextualBehaviorPredIMM < InteractiveMultiModelFilter 24 | properties 25 | % context vector = 6 distances around and 2 velocities adj lanes of 26 | % following vehicles 27 | context 28 | % aggressive and passive driver probabilities [P(d1); P(d2)]; 29 | driverTypes 30 | % driver thresholds - current lane threshold, next lane dist 31 | % threshold and next lane velocity threshold representing diff 32 | % between self vel & follow veh velocity. Vf - Vr < -1 (for a 33 | % passive driver), Vf - Vr < 3 for aggressive driver. This means 34 | % that aggressive driver is willing to accept a lane change gap 35 | % even if the vehicle following in that lane has a higher speed to 36 | % cut in. 37 | driverThresholds 38 | % gap acceptance probabilities [P(G = 0); P(G = 1)] 39 | gapAcceptance 40 | driver1GapAcceptance 41 | driver2GapAcceptance 42 | max_no_of_states 43 | no_of_models 44 | prediction_interval 45 | 46 | end 47 | methods 48 | function self = SLContextualBehaviorPredIMM(Ts) 49 | self@InteractiveMultiModelFilter(Ts); 50 | mm = ZeroAccelerationAndLateralVelMotionModel(Ts); 51 | 52 | % process noise covariance 53 | no_of_states = length(mm.states); 54 | self.no_of_states = no_of_states; 55 | Q = eye(no_of_states) * 0.01; 56 | 57 | % measurement noise covariance 58 | R = [0.0025 0; 0 0.0025]; 59 | % R = [0.0064 0; 0 0.0064]; 60 | % R = [0.01 0; 0 0.01]; 61 | 62 | % added const velocity motion model 63 | flt1 = XKalmanPredictor(Ts,mm); 64 | flt1.updateNoiseStatistics(Q, R); 65 | 66 | mm = ConstantAccelerationZeroLateralVelMotionModel(Ts); 67 | flt2 = XKalmanPredictor(Ts,mm); 68 | flt2.updateNoiseStatistics(Q, R); 69 | 70 | maneuver_length = 100; 71 | lane_center_to_center_distance = 3.5; %meters 72 | mm = LeftLaneChangeRelativeMotionModelWithAcc(Ts,maneuver_length, lane_center_to_center_distance); 73 | flt3 = XKalmanPredictor(Ts,mm); 74 | Ql = diag([diag(Q)' 1]); 75 | flt3.updateNoiseStatistics(Ql, R); 76 | 77 | maneuver_length = 70; 78 | mm = LeftLaneChangeRelativeMotionModelWithAcc(Ts,maneuver_length, lane_center_to_center_distance); 79 | flt4 = XKalmanPredictor(Ts,mm); 80 | flt4.updateNoiseStatistics(Ql, R); 81 | 82 | self.elementalFilters = {flt1, flt2, flt3, flt4}; 83 | self.resetFilterWeights(); 84 | 85 | X0 = [0 10 0 0 0 0]'; 86 | P0 = eye(length(X0))*0.001; 87 | self.setInitialConditions(X0, P0); 88 | 89 | % filter order st, acc, ll 90 | self.markov_transition_matrix = [0.97 0.01 0.01 0.01; ... 91 | 0.001 0.97 0.001 0.028; ... 92 | 0.028 0.001 0.97 0.001; ... 93 | 0.028 0.001 0.001 0.97]; 94 | 95 | self.normalizers = zeros(length(self.elementalFilters), 1); 96 | self.context = zeros(6,1); 97 | 98 | % start with 2 drivers - aggressive and passive by default 99 | self.driverTypes = [0.5, 0.5]; 100 | self.driverThresholds = zeros(2, 3); 101 | self.gapAcceptance = [1; 0]; 102 | self.driver1GapAcceptance = [1;0]; 103 | self.driver2GapAcceptance = [1;0]; 104 | 105 | self.no_of_models = length(self.elementalFilters); 106 | max_states = 0; 107 | for i = 1:length(self.elementalFilters) 108 | flt = self.elementalFilters{i}; 109 | if max_states < flt.no_of_states 110 | max_states = flt.no_of_states; 111 | end 112 | end 113 | self.max_no_of_states = max_states; 114 | self.prediction_interval = 5; 115 | 116 | end 117 | 118 | function pred = getPredictions(self) 119 | no_of_predictions = self.prediction_interval/self.Ts; % 5 / Ts 120 | % no_of_states, time_steps, no_of_filters 121 | pred = zeros(self.max_no_of_states,no_of_predictions,self.no_of_models); 122 | for i = 1: self.no_of_models 123 | flt = self.elementalFilters{i}; 124 | test = squeeze(flt.predictions_state); 125 | pred(1:flt.no_of_states, 1:no_of_predictions, i) ... 126 | = test(1:flt.no_of_states, 1:no_of_predictions); 127 | end 128 | end 129 | 130 | 131 | function setInitialConditions(self, X, P) 132 | for i = 1:length(self.elementalFilters) 133 | flt = self.elementalFilters{i}; 134 | if i == 3 || i == 4 135 | X_aug = [X;0]; 136 | P_aug = diag([diag(P)' 1]); 137 | flt.setInitialConditions(X_aug,P_aug); 138 | else 139 | flt.setInitialConditions(X, P); 140 | end 141 | end 142 | end 143 | 144 | function extractContext(self, participantStates) 145 | % This is the helper function for extracting the context. 146 | % PArticipant states should be the states of all the traffic 147 | % participants. The context vector needs to be extracted from 148 | % it. For now this can simply be context 149 | % context vector = 6 distances around 150 | self.context = participantStates; 151 | end 152 | 153 | function gap = gapAcceptanceDriverPolicy(self, threshold, c) 154 | %gap = [ P(G=0; P(G=1)] 155 | self_vel = self.combined_estimate(2); 156 | % perhaps c(4) is not needed here, threshold(1) is current lane 157 | % threshold 158 | laneSatisfaction = (c(3)) > threshold(1); 159 | % threshold(2) is adjoining lane rear distance threshold. 160 | % threshold(3) is diff in velocity with the foll vehicle 161 | % threshold 162 | leftLaneDissatisfaction = (abs(c(2)) <= threshold(2)) ... 163 | | ((c(2) > threshold(2)) & (c(7)-self_vel) >= threshold(3)); 164 | 165 | %For now since we do not have a right lane, lets keep this as 166 | %1, i.e condition dissatisfied. So gap will be rejected if 167 | %left lane gap requirement is also dissatisfied. If we do not 168 | %assume this for the right lane, then if the lanesatisfaction 169 | %is 0, then the rightlane dissatisfaction will never be false 170 | %as there are no vehicles over there, and hence the gap will 171 | %always be accepted, even if there is a vehicle in the left 172 | %lane. 173 | % TODO: uncomment the following line in multilane scenarios. 174 | % rightLaneDissatisfaction = (c(5) + c(6)) <= threshold; 175 | rightLaneDissatisfaction = true; 176 | gap_rejected = double(laneSatisfaction | (leftLaneDissatisfaction & rightLaneDissatisfaction)); 177 | gap = [gap_rejected; 1 - gap_rejected]; 178 | end 179 | 180 | function gapAcceptancePolicy(self, c) 181 | if nargin == 1 182 | c = self.context; 183 | end 184 | % aggressive 185 | d1 = self.driverTypes(1); 186 | % passive 187 | d2 = self.driverTypes(2); 188 | gapAcceptAgg = self.gapAcceptanceDriverPolicy(self.driverThresholds(1,:), c); 189 | gapAcceptPass = self.gapAcceptanceDriverPolicy(self.driverThresholds(2,:), c); 190 | self.gapAcceptance = d1 * gapAcceptAgg + d2 * gapAcceptPass; 191 | normalizer = sum(self.gapAcceptance); 192 | self.gapAcceptance = self.gapAcceptance/normalizer; 193 | self.driver1GapAcceptance = gapAcceptAgg; 194 | self.driver2GapAcceptance = gapAcceptPass; 195 | end 196 | 197 | % Following is needed only if you are choosing bet left and right 198 | function prob = leftLaneChangeManeuverFunction(self) 199 | c = self.context; 200 | num = c(1) + c(2); 201 | den = num + c(5) + c(6); 202 | % return c1 + c2 / (c1 + c2 + c5 + c6); 203 | prob = num/den; 204 | end 205 | 206 | function prob = rightLaneChangeManeuverFunction(self) 207 | c = self.context; 208 | num = c(5) + c(6); 209 | den = num + c(1) + c(2); 210 | % return c5 + c6 / (c1 + c2 + c5 + c6); 211 | prob = num/den; 212 | end 213 | 214 | function calculateBehaviorProbabilityTransitionMatrix(self) 215 | d1 = self.driverTypes(1); 216 | d2 = self.driverTypes(2); 217 | g_na = self.gapAcceptance(1); 218 | g_a = self.gapAcceptance(2); 219 | 220 | if (g_na < 0.1) 221 | % If g_na is very small, then add 0.01 to it, as there is 1 222 | % in 100 chance (assuming about 10 secs of maneuver length) 223 | % that the maneuver will be complete and the participant 224 | % will switch to default maneuver. If g_na is bigger than 225 | % 0.1, then the influence of this chance can be neglected. 226 | % Of course, if g_na is adjusted by 0.01, g_a will have to 227 | % adjusted too 228 | g_na = g_na + 0.01; 229 | g_a = 1 - g_na; 230 | end 231 | 232 | den = g_na + d2*g_a + d1*g_a; 233 | beh_prob_trans_matrix = [0.97 0.03*g_na/den 0.03*d2*g_a/den 0.03*d1*g_a/den; 234 | (1-0.97)*g_na 0.97 0 (1-0.97)*g_a; 235 | 0.03 0 0.97 0; 236 | 0 0.03 0 0.97]; 237 | 238 | % Normalize the matrix 239 | normalizers = sum(beh_prob_trans_matrix'); 240 | n = length(self.elementalFilters); 241 | normalized_ptm = zeros(size(beh_prob_trans_matrix)); 242 | for i = 1: n 243 | normalized_ptm(i,:) = beh_prob_trans_matrix(i,:) ./ normalizers(i); 244 | end 245 | 246 | self.markov_transition_matrix = normalized_ptm; 247 | end 248 | 249 | function driverUpdate(self) 250 | % this should be called after updating the probabilistic 251 | % weights of all the behaviors (i.e. elemental filters) 252 | w = self.getFilterWeights(); 253 | d1 = w(2) + w(4); 254 | d2 = w(1) + w(3); 255 | updated_driver_weights = [d1 d2]; 256 | normalizer = d1 + d2; 257 | self.driverTypes = updated_driver_weights/normalizer; 258 | end 259 | 260 | function [comb_est, comb_P] = calculateCombinedEstimate(self) 261 | comb_est = [0 0 0 0 0 0]'; 262 | for i = 1: length(self.elementalFilters) 263 | flt = self.elementalFilters{i}; 264 | estimate = flt.estimatedState(1:self.no_of_states); 265 | comb_est = comb_est + flt.weight * estimate; 266 | end 267 | self.combined_estimate = comb_est; 268 | 269 | % calculate combined covariance 270 | 271 | comb_P = zeros(6); 272 | for i = 1: length(self.elementalFilters) 273 | flt = self.elementalFilters{i}; 274 | estimate = flt.estimatedState(1:self.no_of_states); 275 | est_P = flt.estimate_P(1:self.no_of_states, 1:self.no_of_states); 276 | err = estimate - comb_est; 277 | comb_P = comb_P + flt.weight * (err * err' + est_P); 278 | end 279 | self.P_combined_estimate = comb_P; 280 | end 281 | 282 | function est = getFilterEstimates(self) 283 | num_filters = length(self.elementalFilters); 284 | est = zeros(6, 1, num_filters); 285 | for i = 1:num_filters 286 | flt = self.elementalFilters{i}; 287 | est(:,:, i) = flt.predicted_state(1:self.no_of_states); 288 | end 289 | end 290 | end 291 | end -------------------------------------------------------------------------------- /motionModelClasses/ConstantAccelerationMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ConstantAccelerationMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ConstantAccelerationMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | 34 | % states are [x, vx ax, y, vy, ay]; 35 | self.Fd_matrix = [1 self.Ts self.Ts^2/2 0 0 0; 36 | 0 1 self.Ts 0 0 0; 37 | 0 0 1 0 0 0; 38 | 0 0 0 1 self.Ts self.Ts^2/2; 39 | 0 0 0 0 1 self.Ts; 40 | 0 0 0 0 0 1]; 41 | self.Bd_matrix = [0; 0; 1; 0; 0; 0] * self.Ts; 42 | 43 | self.Cd_matrix = [ 1 0 0 0 0 0; 44 | 0 0 0 1 0 0]; 45 | self.Dd_matrix = [0; 0]; 46 | %states [x vx ax y vy ay]'; 47 | self.states = [0 0 0 0 0 0]'; 48 | self.output_states = [ 0 0]'; 49 | 50 | end 51 | end 52 | end -------------------------------------------------------------------------------- /motionModelClasses/ConstantAccelerationZeroLateralVelMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ConstantAccelerationZeroLateralVelMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ConstantAccelerationZeroLateralVelMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | 34 | % states are [x, vx ax, y, vy, ay]; 35 | self.Fd_matrix = [1 self.Ts self.Ts^2/2 0 0 0; 36 | 0 1 self.Ts 0 0 0; 37 | 0 0 1 0 0 0; 38 | 0 0 0 1 self.Ts self.Ts^2/2; 39 | 0 0 0 0 0 0; 40 | 0 0 0 0 0 0]; 41 | self.Bd_matrix = [0; 0; 1; 0; 0; 0] * self.Ts; 42 | 43 | self.Cd_matrix = [ 1 0 0 0 0 0; 44 | 0 0 0 1 0 0]; 45 | self.Dd_matrix = [0; 0]; 46 | %states [x vx ax y vy ay]'; 47 | self.states = [0 0 0 0 0 0]'; 48 | self.output_states = [ 0 0]'; 49 | 50 | end 51 | end 52 | end -------------------------------------------------------------------------------- /motionModelClasses/ConstantLongVelocityStatLateralMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ConstantLongVelocityStatLateralMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ConstantLongVelocityStatLateralMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | self.Fd_matrix = [1 self.Ts 0 0; 34 | 0 1 0 0; 35 | 0 0 1 self.Ts; 36 | 0 0 0 0]; 37 | self.Bd_matrix = [0; 0; 0; 0]; 38 | 39 | self.Cd_matrix = [ 1 0 0 0; 40 | 0 0 1 0]; 41 | self.Dd_matrix = [0; 0]; 42 | %states [x vx y vy]'; 43 | self.states = [0 0 0 0]'; 44 | self.output_states = [ 0 0]'; 45 | end 46 | end 47 | end -------------------------------------------------------------------------------- /motionModelClasses/ConstantVelocityMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ConstantVelocityMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ConstantVelocityMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | self.Fd_matrix = [1 self.Ts 0 0; 34 | 0 1 0 0; 35 | 0 0 1 self.Ts; 36 | 0 0 0 1]; 37 | self.Bd_matrix = [0; 0; 0; 0]; 38 | 39 | self.Cd_matrix = [ 1 0 0 0; 40 | 0 0 1 0]; 41 | self.Dd_matrix = [0; 0]; 42 | %states [x vx y vy]'; 43 | self.states = [0 0 0 0]'; 44 | self.output_states = [ 0 0]'; 45 | end 46 | end 47 | end -------------------------------------------------------------------------------- /motionModelClasses/KinematicBicycleModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef KinematicBicycleModel < NonLinearMotionModel 24 | properties 25 | % states are [x, y, psi, v, beta], v is the velocity vector in 26 | % % inertial frame 27 | init_states 28 | % A matrix in case of linear systems, jacobian matrix F in case of 29 | % non linear 30 | %discretized F matrix (A) matrix 31 | vehicleParameters 32 | end 33 | 34 | methods 35 | function obj = KinematicBicycleModel(Ts, vp, V) 36 | if nargin == 0 37 | % Init to a default vehicle 38 | V = 10; 39 | vp = SedanBicycleModelParameters(); 40 | Ts = 0.1; 41 | elseif nargin == 1 42 | vp = SedanBicycleModelParameters(); 43 | V = 10; 44 | elseif nargin == 2 45 | vp = SedanBicycleModelParameters(); 46 | end 47 | 48 | obj.Ts = Ts; 49 | 50 | % states - [x y psi v beta] 51 | obj.init_states = [0; 0; 0; V; 0]; 52 | obj.states = obj.init_states; 53 | obj.vehicleParameters = vp; 54 | obj.Bd_matrix = [ 0 0; 55 | 0 0; 56 | 0 0; 57 | 1 0; 58 | 0 1] * obj.Ts; 59 | 60 | obj.Cd_matrix = [1 0 0 0 0; 61 | 0 1 0 0 0]; 62 | 63 | obj.Dd_matrix = 0; 64 | 65 | end 66 | 67 | function F = jacobian(obj, X, u) 68 | x = X(1); 69 | y = X(2); 70 | psi = X(3); 71 | v = X(4); 72 | beta = X(5); 73 | l_r = obj.vehicleParameters.l_r; 74 | sin_term = sin(psi+beta); 75 | cos_term = cos(psi+beta); 76 | 77 | F = [ 0 0 -v*sin_term cos_term -v*sin_term ; ... 78 | 0 0 v*cos_term sin_term v*cos_term ; ... 79 | 0 0 0 sin(beta)/l_r v*cos(beta)/l_r ; ... 80 | 0 0 0 0 0 ; ... 81 | 0 0 0 0 0 ]; 82 | end 83 | 84 | function x_apriori = propagate(obj, x, u) 85 | % input u is [a, delta], delta needs to be converted to slip 86 | % (beta) 87 | vp = obj.vehicleParameters; 88 | delta = u(2); 89 | input_beta = atan(vp.l_r/(vp.l_r + vp.l_f)*tan(delta)); 90 | 91 | % Fd = obj.linearizedDiscreteStateTransitionMatrix(x, u); 92 | % x_apriori = Fd * x + obj.Bd_matrix*[u(1); input_beta]; 93 | % states = [x y psi v beta] 94 | % x' = v cos(psi + beta) 95 | % y' = v sin(psi + beta) 96 | % psi' = v sin(beta)/lr 97 | % v' = a = u(1) 98 | % beta' = arctan (lr/(lf+lr) tan(delta)) 99 | x_dot = [ x(4) * cos(x(3)+x(5)); ... 100 | x(4) * sin(x(3)+x(5)); ... 101 | x(4) * sin(x(5))/vp.l_r; ... 102 | u(1) ; ... 103 | input_beta ]; 104 | 105 | x_apriori = x + x_dot * obj.Ts; 106 | obj.propagated_states = x_apriori; 107 | end 108 | 109 | % following should return a jacobian in case of non linear motion 110 | % models 111 | function F = linearizedDiscreteStateTransitionMatrix(obj, x, u) 112 | obj.calculateDiscretizedMatrices(x, u); 113 | F = obj.Fd_matrix; 114 | 115 | end 116 | 117 | function calculateDiscretizedMatrices(obj, x, u) 118 | jacob = obj.jacobian(x, u); 119 | % Non linear system. with EKF, better to keep C and D matrix 120 | % unchanged, so euler forward discretization would be 121 | % preferable. 122 | obj.Fd_matrix = expm(jacob*obj.Ts); 123 | 124 | % B, D and C remain unchanged 125 | 126 | % trap_term = eye(5) - jacob * obj.Ts/2; 127 | % obj.Fd_matrix = trap_term\(eye(5) + jacob * obj.Ts/2); 128 | % obj.Bd_matrix = trap_term\B * obj.Ts; 129 | % obj.Cd_matrix = C/trap_term; 130 | % obj.Dd_matrix = D + obj.Cd_matrix*sysB*stime/2; 131 | 132 | end 133 | 134 | function y_hat = estimatedMeasurement(obj, x_minus) 135 | end 136 | end 137 | end -------------------------------------------------------------------------------- /motionModelClasses/LeftLaneChangeRelXYMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef LeftLaneChangeRelXYMotionModel < LeftLaneChangeRelativeMotionModel 24 | properties 25 | % Maneuver length 26 | L 27 | % Lane width 28 | Wl 29 | % Maneuver amplitude 30 | man_A 31 | % Maneuver frequency calculated from length, w = 2*Pi*1/L = Pi/L 32 | man_w 33 | % init y 34 | current_lane_y 35 | end 36 | 37 | methods 38 | function self = LeftLaneChangeRelXYMotionModel(Ts, maneuver_length, lane_width) 39 | self@LeftLaneChangeRelativeMotionModel(Ts, maneuver_length, lane_width) 40 | end 41 | 42 | function x_plus = propagate(self, x, u) 43 | % states are [x, vx, y, vy, x_mid], where x_mid is point of 44 | % maneuver initiation 45 | % y propagation = -A cos(wx - x_mid) + A; 46 | delta_x = x(1) - x(5); 47 | x_plus = [x(1) + x(2)*self.Ts; ... 48 | x(2); ... 49 | -self.man_A * cos(self.man_w*delta_x) + self.man_A + self.current_lane_y; ... 50 | self.man_A*self.man_w*x(2)*sin(self.man_w*delta_x); ... 51 | x(5)] + u; 52 | self.propagated_states = x_plus; 53 | 54 | end 55 | 56 | function reset(self, x) 57 | self.current_lane_y = x(3); 58 | end 59 | 60 | end 61 | end -------------------------------------------------------------------------------- /motionModelClasses/LeftLaneChangeRelativeMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef LeftLaneChangeRelativeMotionModel < NonLinearMotionModel & MeasurementModel 24 | properties 25 | % Maneuver length 26 | L 27 | % Lane width 28 | Wl 29 | % Maneuver amplitude 30 | man_A 31 | % Maneuver frequency calculated from length, w = 2*Pi*1/L = Pi/L 32 | man_w 33 | % init y 34 | current_lane_y 35 | 36 | end 37 | 38 | methods 39 | function self = LeftLaneChangeRelativeMotionModel(Ts, maneuver_length, lane_width) 40 | if nargin == 0 41 | Ts = 0.1; 42 | maneuver_length = 10; 43 | lane_width = 3.5; 44 | elseif nargin == 1 45 | maneuver_length = 10; 46 | lane_width = 3.5; 47 | elseif nargin == 2 48 | lane_width = 3.5; 49 | end 50 | 51 | self.Ts = Ts; 52 | self.L = maneuver_length; 53 | self.Wl = lane_width; 54 | self.man_A = lane_width/2; 55 | self.man_w = pi/maneuver_length; 56 | 57 | self.Bd_matrix = [0; 0; 0; 0; 0]; 58 | self.Cd_matrix = [1 0 0 0 0; 0 0 1 0 0]; 59 | self.Dd_matrix = [0;0]; 60 | 61 | %states [x vx y vy rel_x]'; 62 | self.states = [0 0 0 0 0]'; 63 | self.output_states = [ 0 0]'; 64 | self.current_lane_y = 0; 65 | end 66 | 67 | function x_plus = propagate(self, x, u) 68 | % states are [x, vx, y, vy, x_mid], where x_mid is point of 69 | % maneuver initiation 70 | % y propagation = -A cos(wx - x_mid) + A; 71 | delta_x = x(1) - x(5); 72 | x_plus = [x(1) + x(2)*self.Ts; ... 73 | x(2); ... 74 | -self.man_A * cos(self.man_w*delta_x) + self.man_A + self.current_lane_y; ... 75 | self.man_A*self.man_w*x(2)*sin(self.man_w*delta_x); ... 76 | x(5)] + u; 77 | self.propagated_states = x_plus; 78 | 79 | end 80 | 81 | function F = linearizedDiscreteStateTransitionMatrix(self, x, u) 82 | % This can call jacobian function if needed 83 | F = self.jacobian(x,u); 84 | end 85 | 86 | function dfdx = jacobian(self, x, u) 87 | % TODO: Correct this jacobian, the following is the linear 88 | % discretized propagation. The jacobian won't have the 1s 89 | delta_x = x(1) - x(5); 90 | dfdx = [ 1 self.Ts 0 0 0; 91 | 0 1 0 0 0; 92 | self.man_A*self.man_w*sin(self.man_w*delta_x) 0 0 0 -self.man_A*self.man_w*sin(self.man_w*delta_x); 93 | self.man_A*self.man_w^2*x(2)*cos(self.man_w*delta_x) self.man_A*self.man_w*sin(self.man_w*delta_x) 0 0 -self.man_A*self.man_w^2*x(2)*cos(self.man_w*delta_x); 94 | 0 0 0 0 1]; 95 | end 96 | 97 | function reset(self, x) 98 | self.current_lane_y = x(3); 99 | end 100 | 101 | end 102 | end -------------------------------------------------------------------------------- /motionModelClasses/LeftLaneChangeRelativeMotionModelWithAcc.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef LeftLaneChangeRelativeMotionModelWithAcc < NonLinearMotionModel & MeasurementModel 24 | properties 25 | % Maneuver length 26 | L 27 | % Lane width 28 | Wl 29 | % Maneuver amplitude 30 | man_A 31 | % Maneuver frequency calculated from length, w = 2*Pi*1/L = Pi/L 32 | man_w 33 | % init y 34 | current_lane_y 35 | 36 | end 37 | 38 | methods 39 | function self = LeftLaneChangeRelativeMotionModelWithAcc(Ts, maneuver_length, lane_width) 40 | if nargin == 0 41 | Ts = 0.1; 42 | maneuver_length = 10; 43 | lane_width = 3.5; 44 | elseif nargin == 1 45 | maneuver_length = 10; 46 | lane_width = 3.5; 47 | elseif nargin == 2 48 | lane_width = 3.5; 49 | end 50 | 51 | self.Ts = Ts; 52 | self.L = maneuver_length; 53 | self.Wl = lane_width; 54 | self.man_A = lane_width/2; 55 | self.man_w = pi/maneuver_length; 56 | 57 | self.Bd_matrix = [0; 0; 1; 0; 0; 0; 0]; 58 | self.Cd_matrix = [1 0 0 0 0 0 0; 0 0 0 1 0 0 0]; 59 | self.Dd_matrix = [0;0]; 60 | 61 | %states [x vx ax y vy ay rel_x]'; 62 | self.states = [0 0 0 0 0 0 0]'; 63 | self.output_states = [ 0 0]'; 64 | self.current_lane_y = 0; 65 | end 66 | 67 | function x_plus = propagate(self, x, u) 68 | % states are [x, vx, ax, y, vy, ay, x_mid], where x_mid is point of 69 | % maneuver initiation 70 | % y propagation = -A cos(wx - x_mid) + A; 71 | delta_x = x(1) - x(7); 72 | x_plus = [x(1) + x(2)*self.Ts + x(3)*self.Ts^2/2; ... 73 | x(2) + self.Ts * x(3); ... 74 | x(3); ... 75 | -self.man_A * cos(self.man_w*delta_x) + self.man_A + self.current_lane_y; ... 76 | self.man_A*self.man_w*x(2)*sin(self.man_w*delta_x); ... 77 | x(3) * self.man_A * self.man_w *sin(self.man_w*delta_x) + self.man_A*self.man_w^2*x(2)^2*cos(self.man_w*delta_x); 78 | x(7)] + self.Bd_matrix * u; 79 | self.propagated_states = x_plus; 80 | end 81 | 82 | function F = linearizedDiscreteStateTransitionMatrix(self, x, u) 83 | % This can call jacobian function if needed 84 | F = self.jacobian(x,u); 85 | end 86 | 87 | function dfdx = jacobian(self, x, u) 88 | % TODO: Correct this jacobian, the following is the linear 89 | % discretized propagation. The jacobian won't have the 1s 90 | delta_x = x(1) - x(7); 91 | awsinwx = self.man_A * self.man_w * sin(self.man_w*delta_x); 92 | awcoswx = self.man_A * self.man_w * cos(self.man_w*delta_x); 93 | aw2coswx = awcoswx * self.man_w; 94 | aw3vsinwx = awsinwx * self.man_w^2 * x(2); 95 | dfdx = [ 1 self.Ts self.Ts^2/2 0 0 0 0; 96 | 0 1 self.Ts 0 0 0 0; 97 | 0 0 1 0 0 0 0; 98 | awsinwx 0 0 0 0 0 -awsinwx; 99 | awcoswx*self.man_w*x(2) awsinwx 0 0 0 0 -self.man_A*self.man_w^2*x(2)*cos(self.man_w*delta_x); 100 | x(3)*aw2coswx-aw3vsinwx*x(2) aw2coswx 2*aw3vsinwx 0 0 0 -x(3)*aw2coswx+aw3vsinwx*x(2); 101 | 0 0 0 0 0 0 1]; 102 | end 103 | 104 | function reset(self, x) 105 | % assign the new y position 106 | self.current_lane_y = x(4); 107 | end 108 | end 109 | end -------------------------------------------------------------------------------- /motionModelClasses/LeftLaneChangeSinusoidalMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef LeftLaneChangeSinusoidalMotionModel < LeftLaneChangeRelativeMotionModel 24 | methods 25 | function self = LeftLaneChangeSinusoidalMotionModel(Ts, maneuver_length, lane_width) 26 | if nargin == 0 27 | Ts = 0.1; 28 | maneuver_length = 10; 29 | lane_width = 3.5; 30 | elseif nargin == 1 31 | maneuver_length = 10; 32 | lane_width = 3.5; 33 | elseif nargin == 2 34 | lane_width = 3.5; 35 | end 36 | % TODO: Unfortunately, following (calling base class 37 | % constructor after the above if statement) isn't supported for 38 | % simulink code generation blocks. Will have to find a work 39 | % around for either the code generation block, or explicitly 40 | % initialize everything else 41 | % self@LeftLaneChangeRelativeMotionModel(Ts, maneuver_length, lane_width); 42 | self.Ts = Ts; 43 | self.L = maneuver_length; 44 | self.Wl = lane_width; 45 | self.man_A = lane_width/2; 46 | self.man_w = pi/maneuver_length; 47 | self.Cd_matrix = [1 0 0 0; 0 0 1 0]; 48 | self.Dd_matrix = [0;0]; 49 | 50 | %states [x vx y vy]'; 51 | self.states = [0 0 0 0]'; 52 | self.output_states = [ 0 0]'; 53 | 54 | end 55 | 56 | function x_plus = propagate(self, x, u) 57 | % states are [x, vx, y, vy] 58 | x_plus = [x(1) + x(2)*self.Ts; ... 59 | x(2); ... 60 | x(3) + x(4)*self.Ts; ... 61 | -self.man_A*self.man_w*x(2)*sin(self.man_w*x(1)-pi)] + u; 62 | self.propagated_states = x_plus; 63 | 64 | end 65 | 66 | function F = linearizedDiscreteStateTransitionMatrix(self, x, u) 67 | % This can call jacobian function if needed 68 | F = self.jacobian(x, u); 69 | end 70 | 71 | function dfdx = jacobian(self, x, u) 72 | % TODO: Correct this jacobian, the following is the linear 73 | % discretized propagation. The jacobian won't have the 1s 74 | dfdx = [1 self.Ts 0 0; ... 75 | 0 1 0 0; ... 76 | 0 0 1 self.Ts; ... 77 | self.man_A*self.man_w^2*x(2)*cos(self.man_w*x(1)-pi) ... 78 | -self.man_A*self.man_w*sin(self.man_w*x(1)-pi) 0 0]; 79 | end 80 | end 81 | end -------------------------------------------------------------------------------- /motionModelClasses/LinearizedBicycleModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef LinearizedBicycleModel < MotionModel & MeasurementModel 24 | properties 25 | % states - [ y y_dot psi psi_dot], input (u) is in radians 26 | init_states 27 | vehicleParameters 28 | a22_c 29 | a24_c 30 | a42_c 31 | a44_c 32 | bb_2 33 | bb_4 34 | V 35 | end 36 | 37 | methods 38 | function obj = LinearizedBicycleModel(Ts, V, vp) 39 | if nargin == 0 40 | % Init to a default vehicle 41 | V = 10; 42 | vp = SedanBicycleModelParameters(); 43 | Ts = 0.1; 44 | elseif nargin == 1 45 | vp = SedanBicycleModelParameters(); 46 | V = 10; 47 | elseif nargin == 2 48 | vp = SedanBicycleModelParameters(); 49 | end 50 | 51 | obj.V = V; 52 | obj.Ts = Ts; 53 | 54 | % use the model from rajamani, with lateral velocity and yaw 55 | % rate as states - [y vy psi psi_dot] 56 | obj.init_states = [0; 0; 0; 0]; 57 | obj.states = obj.init_states; 58 | 59 | obj.a22_c = -(vp.C_f + vp.C_r)/vp.m; 60 | obj.a24_c = (vp.C_f*vp.l_f - vp.C_r*vp.l_r)/vp.m; 61 | obj.a42_c = - (vp.C_f*vp.l_f - vp.C_r * vp.l_r)/vp.Jz; 62 | obj.a44_c = - (vp.l_f^2 * vp.C_f + vp.l_r^2 * vp.C_r)/vp.Jz; 63 | obj.vehicleParameters = vp; 64 | obj.bb_2 = vp.C_f/vp.m; 65 | obj.bb_4 = vp.C_f*vp.l_f/vp.Jz; 66 | 67 | obj.calculateDiscretizedMatrices(V); 68 | 69 | end 70 | 71 | function calculateDiscretizedMatrices(obj, V) 72 | F = obj.calculateContinuousStateTransitionMatrix(V); 73 | B = obj.getInputMatrix(); 74 | C = obj.getOutputMatrix(); 75 | D = 0; 76 | 77 | % disretize using Numerical integration. Ad = expm(A * Ts), 78 | % Bd = (Ad - I)BA^-1 79 | % the problem is the matrix is non invertible 80 | % obj.Fd_matrix = expm(F * obj.Ts); 81 | % 82 | % 83 | % obj.Bd_matrix = (obj.Fd_matrix - eye(4))* F\B; 84 | 85 | % Using euler forward 86 | % obj.Fd_matrix = (eye(4) + F * obj.Ts); 87 | % obj.Bd_matrix = B * obj.Ts; 88 | 89 | % trapezoidal, works good for linear 90 | trap_term = eye(4) - F * obj.Ts/2; 91 | obj.Fd_matrix = trap_term\(eye(4) + F * obj.Ts/2); 92 | obj.Bd_matrix = trap_term\B * obj.Ts; 93 | obj.Cd_matrix = C/trap_term; 94 | obj.Dd_matrix = D + obj.Cd_matrix*B*obj.Ts/2; 95 | obj.V = V; 96 | 97 | end 98 | 99 | function F = calculateContinuousStateTransitionMatrix(obj, Vx) 100 | 101 | % v_dot = -(c1+c2)/(mV^2)v-1/mV(mV+(ac1-bc2)/V)r + c1/(mV) delta 102 | % r_dot = (-ac1+bc2)/(JzV)v-(a^2C1+b^2C2)/(JzV)r + (ac1/Jz)delta 103 | % Its important to leave only in terms of V as a variable. 104 | % i.e., calculate all the other coefficients axx_c beforehand, 105 | % and then substitute V at runtime. 106 | a22 = obj.a22_c/Vx; 107 | a24 = -Vx - obj.a24_c/Vx; 108 | a42 = obj.a42_c/Vx; 109 | a44 = obj.a44_c/Vx; 110 | F = [0 1 0 0; ... 111 | 0 a22 0 a24; ... 112 | 0 0 0 1; ... 113 | 0 a42 0 a44]; 114 | end 115 | 116 | function B = getInputMatrix(obj) 117 | B = [0; obj.bb_2; 0; obj.bb_4]; 118 | end 119 | 120 | function C = getOutputMatrix(obj) 121 | C = [1 0 0 0 ]; 122 | end 123 | 124 | end 125 | end -------------------------------------------------------------------------------- /motionModelClasses/MeasurementModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef MeasurementModel < handle 24 | properties 25 | %output states 26 | output_states 27 | 28 | end 29 | 30 | methods 31 | function y_hat = estimatedMeasurement(obj, x_minus, u) 32 | % x_minus should typically be the predicted estimate, a column 33 | % vector 34 | y_hat = obj.Cd_matrix * x_minus + obj.Dd_matrix * u; 35 | end 36 | 37 | end 38 | end -------------------------------------------------------------------------------- /motionModelClasses/MildAccelerationZeroLateralVelMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef MildAccelerationZeroLateralVelMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = MildAccelerationZeroLateralVelMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | 34 | % states are [x, vx ax, y, vy, ay]; 35 | self.Fd_matrix = [1 self.Ts self.Ts^2/2 0 0 0; 36 | 0 1 self.Ts 0 0 0; 37 | 0 0 1 0 0 0; 38 | 0 0 0 1 self.Ts self.Ts^2/2; 39 | 0 0 0 0 0 self.Ts; 40 | 0 0 0 0 0 0]; 41 | self.Bd_matrix = [0; 0; 1; 0; 0; 0] * self.Ts; 42 | 43 | self.Cd_matrix = [ 1 0 0 0 0 0; 44 | 0 0 0 1 0 0]; 45 | self.Dd_matrix = [0; 0]; 46 | %states [x vx ax y vy ay]'; 47 | self.states = [0 0 0 0 0 0]'; 48 | self.output_states = [ 0 0]'; 49 | 50 | end 51 | 52 | function x_apriori = propagate(obj, x, u) 53 | % u is in radians 54 | % following should be set only by updates 55 | % obj.states = x; 56 | % 1m/s acceleration 57 | x_apriori = obj.Fd_matrix * x + obj.Bd_matrix * 1; 58 | obj.propagated_states = x_apriori; 59 | end 60 | end 61 | end -------------------------------------------------------------------------------- /motionModelClasses/MotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef MotionModel < handle %& matlab.mixin.Copyable 24 | properties 25 | Ts 26 | states 27 | propagated_states 28 | 29 | % discretized matrices 30 | Fd_matrix 31 | Bd_matrix 32 | Cd_matrix 33 | Dd_matrix 34 | end 35 | methods (Abstract) 36 | % x_apriori = propagate(obj, x, u) 37 | 38 | % following should return a jacobian in case of non linear motion 39 | % models 40 | % F = linearizedDiscreteStateTransitionMatrix(obj, x, u) 41 | end 42 | 43 | methods 44 | function x_apriori = propagate(obj, x, u) 45 | % u is in radians 46 | % following should be set only by updates 47 | % obj.states = x; 48 | x_apriori = obj.Fd_matrix * x + obj.Bd_matrix * u; 49 | obj.propagated_states = x_apriori; 50 | end 51 | 52 | function Fd = linearizedDiscreteStateTransitionMatrix(obj, x, u) 53 | % x not used since the system is linear 54 | Fd = obj.Fd_matrix; 55 | end 56 | 57 | function c_matrix = getOutputMatrix(self) 58 | c_matrix = self.Cd_matrix; 59 | end 60 | 61 | function setStates(self, x) 62 | self.states = x; 63 | end 64 | 65 | function reset(self, x) 66 | end 67 | end 68 | end -------------------------------------------------------------------------------- /motionModelClasses/NonLinearMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef NonLinearMotionModel < MotionModel 24 | methods(Abstract) 25 | F = jacobian(obj, x, u) 26 | end 27 | end -------------------------------------------------------------------------------- /motionModelClasses/RightLaneChangeRelativeMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef RightLaneChangeRelativeMotionModel < LeftLaneChangeRelativeMotionModel 24 | properties 25 | 26 | end 27 | 28 | methods 29 | function self = RightLaneChangeRelativeMotionModel(Ts, maneuver_length, lane_width) 30 | self@LeftLaneChangeRelativeMotionModel(Ts, maneuver_length, lane_width); 31 | end 32 | 33 | function x_plus = propagate(self, x, u) 34 | % states are [x, vx, y, vy, x_mid], where x_mid is point of 35 | % maneuver initiation 36 | % Right lane change 37 | % y propagation = A cos(wx - x_mid) - A + y_init; 38 | delta_x = x(1) - x(5); 39 | x_plus = [x(1) + x(2)*self.Ts; ... 40 | x(2); ... 41 | self.man_A * cos(self.man_w*delta_x) - self.man_A + self.current_lane_y; ... 42 | -self.man_A*self.man_w*x(2)*sin(self.man_w*delta_x); ... 43 | x(5)] + u; 44 | self.propagated_states = x_plus; 45 | 46 | end 47 | 48 | function dfdx = jacobian(self, x, u) 49 | % TODO: Correct this jacobian, the following is the linear 50 | % discretized propagation. The jacobian won't have the 1s 51 | delta_x = x(1) - x(5); 52 | dfdx = [ 1 self.Ts 0 0 0; 53 | 0 1 0 0 0; 54 | -self.man_A*self.man_w*sin(self.man_w*delta_x) 0 0 0 self.man_A*self.man_w*sin(self.man_w*delta_x); 55 | -self.man_A*self.man_w^2*x(2)*cos(self.man_w*delta_x) -self.man_A*self.man_w*sin(self.man_w*delta_x) 0 0 self.man_A*self.man_w^2*x(2)*cos(self.man_w*delta_x); 56 | 0 0 0 0 1]; 57 | end 58 | end 59 | end -------------------------------------------------------------------------------- /motionModelClasses/RightLaneChangeSinusoidalMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef RightLaneChangeSinusoidalMotionModel < LeftLaneChangeSinusoidalMotionModel 24 | methods 25 | function self = RightLaneChangeSinusoidalMotionModel(Ts, maneuver_length, lane_width) 26 | self@LeftLaneChangeSinusoidalMotionModel(Ts, maneuver_length, lane_width); 27 | % self.Ts = Ts; 28 | % self.L = maneuver_length; 29 | % self.Wl = lane_width; 30 | % self.man_A = lane_width/2; 31 | % self.man_w = pi/maneuver_length; 32 | % self.Cd_matrix = [1 0 0 0; 0 0 1 0]; 33 | % self.Dd_matrix = [0;0]; 34 | end 35 | 36 | function x_plus = propagate(self, x, u) 37 | % states are [x, vx, y, vy] 38 | % x propagation: linear constant velocity 39 | % y propagation: y = A Cos (wx) -A, v = -Awv Sin(wx) 40 | x_plus = [x(1) + x(2)*self.Ts; ... 41 | x(2); ... 42 | x(3) + x(4)*self.Ts; ... 43 | -self.man_A*self.man_w*x(2)*sin(self.man_w*x(1))] + u; 44 | self.propagated_states = x_plus; 45 | end 46 | 47 | % following is called from linearizedDiscreteStateTransitionMatrix 48 | % from the base class 49 | function dfdx = jacobian(self, x, u) 50 | % TODO: Correct this jacobian, the following is the linear 51 | % discretized propagation. The jacobian won't have the 1s 52 | dfdx = [1 self.Ts 0 0; ... 53 | 0 1 0 0; ... 54 | 0 0 1 self.Ts; ... 55 | self.man_A*self.man_w^2*x(2)*cos(self.man_w*x(1)) ... 56 | -self.man_A*self.man_w*sin(self.man_w*x(1)) 0 0]; 57 | end 58 | end 59 | end -------------------------------------------------------------------------------- /motionModelClasses/SimplifiedKinematicBicycleModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef SimplifiedKinematicBicycleModel < NonLinearMotionModel & MeasurementModel 24 | properties 25 | init_states 26 | vehicleParameters 27 | end 28 | 29 | methods 30 | function obj = SimplifiedKinematicBicycleModel(Ts, vp, V) 31 | if nargin == 0 32 | % Init to a default vehicle 33 | V = 10; 34 | vp = SedanBicycleModelParameters(); 35 | Ts = 0.1; 36 | elseif nargin == 1 37 | vp = SedanBicycleModelParameters(); 38 | V = 10; 39 | elseif nargin == 2 40 | vp = SedanBicycleModelParameters(); 41 | end 42 | 43 | obj.Ts = Ts; 44 | 45 | % states - [x y psi v beta] 46 | obj.init_states = [0; 0; 0; V; 0]; 47 | obj.states = obj.init_states; 48 | obj.vehicleParameters = vp; 49 | 50 | obj.Bd_matrix = [ 0 0; 51 | 0 0; 52 | 0 0; 53 | 1 0; 54 | 0 1] * obj.Ts; 55 | 56 | obj.Cd_matrix = [1 0 0 0 0; 57 | 0 1 0 0 0]; 58 | 59 | obj.Dd_matrix = 0; 60 | end 61 | 62 | function F = jacobian(obj, X, u) 63 | x = X(1); 64 | y = X(2); 65 | psi = X(3); 66 | v = X(4); 67 | delta = X(5); 68 | l = obj.vehicleParameters.l_r + obj.vehicleParameters.l_f; 69 | sin_term = sin(psi); 70 | cos_term = cos(psi); 71 | 72 | F = [ 0 0 -v*sin_term cos_term 0 ; ... 73 | 0 0 v*cos_term sin_term 0 ; ... 74 | 0 0 0 tan(delta)/l v*(sec(delta)^2)/l; ... 75 | 0 0 0 0 0 ; ... 76 | 0 0 0 0 0 ]; 77 | end 78 | 79 | function x_apriori = propagate(obj, x, u) 80 | % input u is [a, omega], 81 | 82 | % [ x y psi v delta] 83 | % x' = v cos(psi) 84 | % y' = v sin(psi) 85 | % psi' = v tan(delta)/l 86 | % v' = a = u(1) 87 | % delta' = w = u(2) 88 | l = obj.vehicleParameters.l_r + obj.vehicleParameters.l_f; 89 | x_dot = [ x(4) * cos(x(3)); 90 | x(4) * sin(x(3)); 91 | x(4) * tan(x(5))/l; 92 | u(1); 93 | u(2) ]; 94 | x_apriori = x + x_dot * obj.Ts; 95 | obj.propagated_states = x_apriori; 96 | end 97 | 98 | % following should return a jacobian in case of non linear motion 99 | % models 100 | function F = linearizedDiscreteStateTransitionMatrix(obj, x, u) 101 | obj.calculateDiscretizedMatrices(x, u); 102 | F = obj.Fd_matrix; 103 | end 104 | 105 | function calculateDiscretizedMatrices(obj, x, u) 106 | jacob = obj.jacobian(x, u); 107 | obj.Fd_matrix = expm(jacob*obj.Ts); 108 | % trap_term = eye(5) - jacob * obj.Ts/2; 109 | % obj.Fd_matrix = trap_term\(eye(5) + jacob * obj.Ts/2); 110 | 111 | %Also update Bd_matrix, since that has changed too 112 | % B = [ 0 0; 113 | % 0 0; 114 | % 0 0; 115 | % 1 0; 116 | % 0 1]; 117 | % 118 | % obj.Bd_matrix = trap_term\B * obj.Ts; 119 | end 120 | 121 | end 122 | end -------------------------------------------------------------------------------- /motionModelClasses/ZeroAccelerationAndLateralVelMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ZeroAccelerationAndLateralVelMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ZeroAccelerationAndLateralVelMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | 34 | % states are [x, vx ax, y, vy, ay]; 35 | self.Fd_matrix = [1 self.Ts self.Ts^2/2 0 0 0; 36 | 0 1 self.Ts 0 0 0; 37 | 0 0 0 0 0 0; 38 | 0 0 0 1 self.Ts self.Ts^2/2; 39 | 0 0 0 0 0 self.Ts; 40 | 0 0 0 0 0 0]; 41 | self.Bd_matrix = [0; 0; 0; 0; 0; 0]; 42 | 43 | self.Cd_matrix = [ 1 0 0 0 0 0; 44 | 0 0 0 1 0 0]; 45 | self.Dd_matrix = [0; 0]; 46 | %states [x vx ax y vy ay]'; 47 | self.states = [0 0 0 0 0 0]'; 48 | self.output_states = [ 0 0]'; 49 | 50 | end 51 | end 52 | end -------------------------------------------------------------------------------- /motionModelClasses/ZeroAccelerationMotionModel.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | classdef ZeroAccelerationMotionModel < MotionModel & MeasurementModel 24 | properties 25 | end 26 | 27 | methods 28 | function self = ZeroAccelerationMotionModel(Ts) 29 | if nargin == 0 30 | Ts = 0.1; 31 | end 32 | self.Ts = Ts; 33 | 34 | % states are [x, vx ax, y, vy, ay]; 35 | self.Fd_matrix = [1 self.Ts self.Ts^2/2 0 0 0; 36 | 0 1 self.Ts 0 0 0; 37 | 0 0 0 0 0 0; 38 | 0 0 0 1 self.Ts self.Ts^2/2; 39 | 0 0 0 0 1 self.Ts; 40 | 0 0 0 0 0 0]; 41 | self.Bd_matrix = [0; 0; 0; 0; 0; 0]; 42 | 43 | self.Cd_matrix = [ 1 0 0 0 0 0; 44 | 0 0 0 1 0 0]; 45 | self.Dd_matrix = [0; 0]; 46 | %states [x vx ax y vy ay]'; 47 | self.states = [0 0 0 0 0 0]'; 48 | self.output_states = [ 0 0]'; 49 | 50 | end 51 | end 52 | end -------------------------------------------------------------------------------- /readme_images/aggressive_driver_scenario.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jollysg/contextual_behavior_prediction/16b550e158f77b7ab64adb185c6879e329a29609/readme_images/aggressive_driver_scenario.png -------------------------------------------------------------------------------- /readme_images/passive_driver_scenario.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jollysg/contextual_behavior_prediction/16b550e158f77b7ab64adb185c6879e329a29609/readme_images/passive_driver_scenario.png -------------------------------------------------------------------------------- /sim_scenarios/contextual_IMM_main.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | % contextual IMM program 24 | 25 | ctx_imm = SLContextualBehaviorPredIMM(Ts_bp); 26 | % thresholds - current lane threshold, next lane dist threshold and next 27 | % lane velocity threshold representing diff between self vel & follow veh 28 | % velocity. Vf - Vr < -1 (for a passive driver), Vf - Vr < 3 for aggressive 29 | % driver. This means that aggressive driver is willing to accept a lane 30 | % change gap even if the vehicle following in that lane has a higher speed 31 | % to cut in. 32 | ctx_imm.driverThresholds = [60 5 3; 60 15 -1]; 33 | ctx_imm.driverTypes = [0.5 0.5]; 34 | 35 | sigma_r = 0.05; 36 | 37 | if aggressive_driver_use_case == true 38 | initial_front_car_distance = 100; 39 | leftLaneFollowingVehicleInitPosition = -106; 40 | else 41 | initial_front_car_distance = 80; 42 | leftLaneFollowingVehicleInitPosition = -25; 43 | end 44 | 45 | for i = 1:length(simtime) 46 | t = simtime(i); 47 | if t < 20 48 | front_car_distance = initial_front_car_distance - 3.33 * t; 49 | end 50 | 51 | % Left lane vehicle position and corresponding context update 52 | leftLaneVehiclePosition = leftLaneFollowingVehicleInitPosition + 15*t; 53 | dist_LV = ctx_imm.combined_estimate(1) - leftLaneVehiclePosition; 54 | no_of_filters = length(ctx_imm.elementalFilters); 55 | 56 | %context vector - first 6 are distances and the next two are 57 | %velcocities with following vehicles in the adjoining lane. 58 | context = [100 100 100 100 100 100 15 15]'; 59 | 60 | %comment following to remove the front vehicle 61 | context(3) = front_car_distance; 62 | 63 | %comment folling to remove the left lane vehicle 64 | context(2) = dist_LV; 65 | if t == 11.9 66 | context(2) = dist_LV; 67 | end 68 | 69 | ctx_imm.extractContext(context); 70 | ctx_imm.gapAcceptancePolicy(); 71 | 72 | ctx_imm.calculateBehaviorProbabilityTransitionMatrix(); 73 | 74 | % mix initial states for the current cycle first 75 | ctx_imm.mixEstimates(); 76 | 77 | filter_traj(i).mark_trans_matrix = ctx_imm.markov_transition_matrix; 78 | filter_traj(i).mix_init_states = ctx_imm.mixed_init_state; 79 | % predict 80 | ctx_imm.predict(0); 81 | 82 | % correct and update probabilistic weights 83 | 84 | if enable_measurement_noise == true 85 | y_tilde = groundTruth(i).y_tilde; 86 | else 87 | y_tilde = groundTruth(i).y_gt; 88 | end 89 | ctx_imm.correct(y_tilde); 90 | 91 | % combined estimates 92 | [comb_x, comb_p] = ctx_imm.calculateCombinedEstimate(); 93 | ctx_imm.driverUpdate(); 94 | 95 | filter_traj(i).prenormalizedWts = ctx_imm.getFilterNonNormalizedWeights()'; 96 | filter_traj(i).weights = ctx_imm.getFilterWeights()'; 97 | filter_traj(i).likelihoods = ctx_imm.getFilterLikelihoods()'; 98 | filter_traj(i).estimates = ctx_imm.getFilterEstimates(); 99 | filter_traj(i).err = ctx_imm.getFilterErrors(); 100 | filter_traj(i).driver_weights = ctx_imm.driverTypes'; 101 | filter_traj(i).combined_estimates = comb_x; 102 | filter_traj(i).combined_p = comb_p; 103 | filter_traj(i).gapAcceptance = ctx_imm.gapAcceptance; 104 | filter_traj(i).context = context; 105 | filter_traj(i).leftLaneVehPosn = leftLaneVehiclePosition; 106 | filter_traj(i).distLLV = dist_LV; 107 | filter_traj(i).d1GapAcceptPolicy = ctx_imm.driver1GapAcceptance; 108 | filter_traj(i).d2GapAcceptPolicy = ctx_imm.driver2GapAcceptance; 109 | filter_traj(i).predictions = ctx_imm.getPredictions(); 110 | filter_traj(i).front_car_position = comb_x(1) + front_car_distance; 111 | end 112 | 113 | post_processing_plots; 114 | -------------------------------------------------------------------------------- /sim_scenarios/ground_truth_and_measurements.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | 24 | Ts_bp = 0.1; 25 | simtime = 0:Ts_bp:25; 26 | 27 | groundTruth = struct.empty(0,length(simtime)); 28 | X_init = [x vx ax y vy ay]'; 29 | no_of_states = length(X_init); 30 | X = X_init; 31 | X_GT = X_init; 32 | filter_traj = struct.empty(0,length(simtime)); 33 | acc = 0; 34 | 35 | for i = 1:length(simtime) 36 | t = simtime(i); 37 | %Following code adds acceleration of 1m/s after 10 seconds for 1 38 | %second. As a result, the vehicle accelerates by 1 m/s after this. 39 | %Comment the following IF condition for a constant velocity run. 40 | % if t >= 10 && t < 11 41 | % acc = 1; 42 | % else 43 | % acc = 0; 44 | % end 45 | [y_gt, X_GT] = generateGroundTruth(X_GT, acc); 46 | [y_tilde, X] = generateMeasurement(X,acc); 47 | groundTruth(i).t = t; 48 | groundTruth(i).gt_states = X_GT; 49 | groundTruth(i).states = X; 50 | groundTruth(i).y_tilde = y_tilde; 51 | groundTruth(i).y_gt = y_gt; 52 | end 53 | 54 | contextual_IMM_main; 55 | 56 | 57 | function [y, perturbed_states] = generateMeasurement(X, u) 58 | Ts_bp = 0.1; 59 | % states - X = [x vx y vy] - 60 | Ak = [ 1 Ts_bp Ts_bp^2/2 0 0 0; 61 | 0 1 Ts_bp 0 0 0; 62 | 0 0 1 0 0 0; 63 | 0 0 0 1 Ts_bp Ts_bp^2/2; 64 | 0 0 0 0 1 Ts_bp ; 65 | 0 0 0 0 0 1]; 66 | Bk = [0; 0; 1; 0; 0; 0] * Ts_bp; 67 | 68 | % X(5) = 0; 69 | X(4) = 0; 70 | 71 | X_GT = Ak * X + Bk * u; 72 | perturbed_states = addProcessNoise(X_GT, Ts_bp); 73 | 74 | y = generateNoisyMeasurement(X_GT); 75 | end 76 | 77 | function x = addProcessNoise(X, Ts_bp) 78 | % process noise std dev 79 | % sigma_matrix = diag([0.16 0.16 0.16 0.16]); 80 | sigma_matrix = diag([0.032 0.032 0.032 0.032 0.032 0.032]); 81 | % sigma_matrix = diag([0 0 0 0 0 0]); 82 | x = X + sqrt(Ts_bp)*sigma_matrix*randn(6,1); 83 | end 84 | 85 | function y = generateNoisyMeasurement(X) 86 | Ck = [1 0 0 0 0 0; 0 0 0 1 0 0]; 87 | 88 | % measurement noise std_dev 89 | % R_sigma = [0.5 0; 0 0.5]; 90 | R_sigma = [0.05 0; 0 0.05]; 91 | 92 | % Measurement noise is being added externally using the random number block 93 | % R_sigma = [0.0 0; 0 0.0]; 94 | y = Ck * X + R_sigma * randn(2,1); 95 | end 96 | 97 | function y = generateNoiselessMeasurement(X) 98 | Ck = [1 0 0 0 0 0; 0 0 0 1 0 0]; 99 | y = Ck * X; 100 | end 101 | function [y, X_GT] = generateGroundTruth(X, u) 102 | Ts_bp = 0.1; 103 | % states - X = [x vx y vy] - 104 | Ak = [ 1 Ts_bp Ts_bp^2/2 0 0 0; 105 | 0 1 Ts_bp 0 0 0; 106 | 0 0 1 0 0 0; 107 | 0 0 0 1 Ts_bp Ts_bp^2/2; 108 | 0 0 0 0 1 Ts_bp ; 109 | 0 0 0 0 0 1]; 110 | Bk = [0; 0; 1; 0; 0; 0] * Ts_bp; 111 | Ck = [1 0 0 0 0 0; 0 0 0 1 0 0]; 112 | 113 | % X(5) = 0; 114 | X(4) = 0; 115 | 116 | X_GT = Ak * X + Bk * u; 117 | y = generateNoiselessMeasurement(X_GT); 118 | end -------------------------------------------------------------------------------- /sim_scenarios/lane_change_noisy_measurements_generator.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | Ts_bp = 0.1; 24 | simtime = 0:Ts_bp:25; 25 | 26 | % set the following true for the aggressive driver scenario, false for the 27 | % passive drive scenario. 28 | aggressive_driver_use_case = false; 29 | 30 | mm1 = ZeroAccelerationAndLateralVelMotionModel(Ts_bp); 31 | mm2 = ConstantAccelerationZeroLateralVelMotionModel(Ts_bp); 32 | 33 | if aggressive_driver_use_case == false 34 | mm3 = LeftLaneChangeRelativeMotionModelWithAcc(Ts_bp, 100, 3.5); 35 | else 36 | mm3 = LeftLaneChangeRelativeMotionModelWithAcc(Ts_bp, 70, 3.5); 37 | 38 | end 39 | 40 | % No acceleration input by default 41 | u = 0; 42 | 43 | % for passive driver 44 | lane_change_initiation_time = 10; 45 | 46 | if aggressive_driver_use_case == true 47 | lane_change_initiation_time = 15; 48 | end 49 | 50 | % Start with constant acceleration 51 | X_aug = [0 10 0 0 0 0]'; 52 | laneChangeMeas = {}; 53 | 54 | % time at which the lane change is complete. This comes out to be approx 55 | % the same for both scenarios and is just a mere coincidence 56 | lc_time = 19.3; 57 | 58 | enable_process_noise = false; 59 | enable_measurement_noise = true; 60 | 61 | for i = 1:length(simtime) 62 | t = simtime(i); 63 | 64 | if aggressive_driver_use_case == true 65 | if t>= 10 && t < 11 66 | % apply 1 m/s2 acceleration as input for 1 second. 67 | u = 0.5; 68 | lc_time = 20.2; 69 | % u = 1; 70 | % lc_time = 19.3; 71 | else 72 | u = 0; 73 | end 74 | end 75 | 76 | if t == lane_change_initiation_time 77 | % store the maneuver start point 78 | x_mid = X_aug(1); 79 | end 80 | 81 | if aggressive_driver_use_case == true 82 | if t < 10 83 | X_aug = mm1.propagate(X_aug, 0); 84 | elseif t >= 10 && t < 15 85 | X_aug = mm2.propagate(X_aug, u); 86 | elseif t >=15 && t < lc_time 87 | X_lc = [X_aug; x_mid]; 88 | X_lc = mm3.propagate(X_lc, u); 89 | X_aug = X_lc(1:6); 90 | elseif t >=lc_time 91 | X_aug = mm2.propagate(X_aug, u); 92 | end 93 | else 94 | if t < 10 95 | X_aug = mm1.propagate(X_aug, 0); 96 | elseif t >=lane_change_initiation_time && t < lc_time 97 | X_lc = [X_aug; x_mid]; 98 | X_lc = mm3.propagate(X_lc, u); 99 | X_aug = X_lc(1:6); 100 | elseif t >=lc_time 101 | X_aug = mm2.propagate(X_aug, u); 102 | end 103 | end 104 | 105 | laneChangeMeas(i).x = X_aug(1); 106 | laneChangeMeas(i).y = X_aug(4); 107 | if enable_process_noise 108 | X_aug = addProcessNoise(X_aug,Ts_bp); 109 | end 110 | laneChangeMeas(i).estimates = X_aug; 111 | end 112 | 113 | groundTruth = struct.empty(0,length(simtime)); 114 | 115 | for i = 1:length(simtime) 116 | t = simtime(i); 117 | groundTruth(i).t = t; 118 | groundTruth(i).gt_states = laneChangeMeas(i).estimates; 119 | groundTruth(i).states = laneChangeMeas(i).estimates; 120 | noisy_meas = generateMeasurement(laneChangeMeas(i).estimates, enable_measurement_noise); 121 | groundTruth(i).y_tilde = noisy_meas; 122 | groundTruth(i).y_gt = [laneChangeMeas(i).x; laneChangeMeas(i).y]; 123 | end 124 | 125 | contextual_IMM_main; 126 | 127 | % debug visualizations for y axis of lane change vs time 128 | % figure(1); 129 | % plot(simtime, [laneChangeMeas(:).y]); 130 | % 131 | 132 | % debug visualizations for y axis of lane change vs x. 133 | % figure(2); 134 | % plot([laneChangeMeas(:).x], [laneChangeMeas(:).y]); 135 | 136 | % figure(3); 137 | % plot(simtime, [laneChangeMeas(:).estimates]); 138 | 139 | 140 | function x = addProcessNoise(X, Ts_bp) 141 | % process noise std dev 142 | % sigma_matrix = diag([0.16 0.16 0.16 0.16]); 143 | % sigma_matrix = diag([0.032 0.032 0.032 0.032 0.032 0.032]); 144 | % sigma_matrix = diag([0.1 0.1 0.1 0.1 0.1 0.1]); 145 | % sigma_matrix = diag([0 0 0 0 0 0]); 146 | sigma_matrix = diag([0.071 0.071 0.071 0.071 0.071 0.071]); 147 | x = X + sqrt(Ts_bp)*sigma_matrix*randn(6,1); 148 | end 149 | 150 | function y = generateMeasurement(X, enable_measurement_noise) 151 | Ck = [1 0 0 0 0 0; 0 0 0 1 0 0]; 152 | 153 | % measurement noise std_dev 154 | % R_sigma = [0.5 0; 0 0.5]; 155 | 156 | if enable_measurement_noise == true 157 | R_sigma = [0.05 0; 0 0.05]; 158 | else 159 | R_sigma = [0.0 0; 0 0.0]; 160 | end 161 | % Measurement noise is being added externally using the random number block 162 | y = Ck * X + R_sigma * randn(2,1); 163 | end 164 | -------------------------------------------------------------------------------- /sim_scenarios/post_processing_plots.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | figure(1) 24 | plot(simtime, [filter_traj(:).driver_weights], 'Linewidth', 2); 25 | legend('Aggr. driver', 'Passive driver'); 26 | legend('Location', 'northwest'); 27 | xlabel('Time (seconds)'); 28 | ylabel('Driver weights (probabilities)'); 29 | title('Driver weights vs time'); 30 | ylim([-0.1 1.19]); 31 | grid on; 32 | 33 | figure(2) 34 | plot(simtime, [filter_traj(:).likelihoods]); 35 | legend('Straight pass', 'straight agg', 'left lane pass', 'left lane agg'); 36 | xlabel('Time (seconds)'); 37 | ylabel('likelihood of filters'); 38 | title('Filter likelihoods vs time'); 39 | lim = ylim; 40 | lim(1) = -5; 41 | ylim(lim) 42 | grid on; 43 | 44 | figure(3) 45 | plot(simtime, [filter_traj(:).weights], 'Linewidth', 1.5); 46 | legend('Straight pass', 'straight agg', 'left lane pass', 'left lane agg'); 47 | legend('Location', 'northwest'); 48 | xlabel('Time (seconds)'); 49 | ylabel('Filter probabilities'); 50 | title('Filter probabilistic weights vs time'); 51 | ylim([-0.1 1.19]); 52 | grid on; 53 | 54 | estimate_flt1 = []; 55 | for i = 1:length(filter_traj) 56 | estimate_flt1(:,i) = squeeze(filter_traj(i).estimates(:,:,1)); 57 | end 58 | 59 | figure(4) 60 | plot(simtime, estimate_flt1); 61 | legend('x', 'vx', 'ax', 'y', 'vy', 'ay'); 62 | legend('Location', 'northwest'); 63 | xlabel('Time (seconds)'); 64 | ylabel('State estimate Filter 1'); 65 | title('State estimates vs time'); 66 | grid on; 67 | 68 | figure(5) 69 | plot(simtime, [filter_traj(:).combined_estimates]); 70 | legend('x', 'vx', 'ax', 'y', 'vy', 'ay'); 71 | xlabel('Time (seconds)'); 72 | ylabel('Combined estimates'); 73 | legend('Location', 'northwest'); 74 | title('Combined estimates vs time'); 75 | grid on; 76 | 77 | figure(6) 78 | combined_estim_traj = [filter_traj(:).combined_estimates]; 79 | y_gt = [groundTruth(:).y_gt]; 80 | meas = [groundTruth(:).y_tilde]; 81 | plot(y_gt(1,:), y_gt(2,:), 'r', ... 82 | meas(1,:), meas(2,:), 'g', ... 83 | combined_estim_traj(1,:), combined_estim_traj(4,:), 'b', ... 84 | 'Linewidth', 1.5); 85 | title('Estimates vs ground truth of position trajectories'); 86 | xlabel('x position (meters)'); 87 | ylabel('y position (meters)'); 88 | legend('ground truth', 'meas', 'estimates'); 89 | legend('Location', 'northwest'); 90 | ylim([-1.75 3.5+1.75]); 91 | 92 | estim_comb = [combined_estim_traj(1,:); combined_estim_traj(4,:)]; 93 | 94 | % Requires signal processing toolbox 95 | % rms_error = rms(estim_comb' - y_gt') 96 | -------------------------------------------------------------------------------- /sim_scenarios/pred_post_processing_matlab_ws_x_coord_mod.m: -------------------------------------------------------------------------------- 1 | % MIT License 2 | % 3 | % Copyright (c) 2020 Jasprit Singh Gill (jaspritsgill@gmail.com) 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | 12 | % The above copyright notice and this permission notice shall be included in all 13 | % copies or substantial portions of the Software. 14 | 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | % SOFTWARE. 22 | 23 | 24 | meas = [groundTruth(:).y_tilde]; 25 | time_ser = [groundTruth(:).t]; 26 | estim = [filter_traj(:).combined_estimates]; 27 | 28 | % predictions - 5x50x5x251 no_of_states, no_of_predictions, no_of_models, 29 | % no_of_time_steps 30 | 31 | %1x5x251 32 | driver_wts = [filter_traj(:).driver_weights]; 33 | wts = [filter_traj(:).weights]; 34 | right_lane_center = zeros(1,length(time_ser)); 35 | left_lane_center = right_lane_center + 3.5; 36 | trail_length = 20; 37 | 38 | lane_reached = false; 39 | max_ys_x = -1; 40 | 41 | for i = 1:length(time_ser) 42 | if mod(i-1,5) == 0 43 | 44 | straight_pass_preds = filter_traj(i).predictions(:,:,1); 45 | straight_agg_preds = filter_traj(i).predictions(:,:,2); 46 | left_lane_passive = filter_traj(i).predictions(:,:,3); 47 | left_lane_aggressive = filter_traj(i).predictions(:,:,4); 48 | 49 | wt1 = wts(1,1:i); 50 | wt2 = wts(2,1:i); 51 | wt3 = wts(3,1:i); 52 | wt4 = wts(4,1:i); 53 | wted_pred = straight_pass_preds * wt1(end) ... 54 | + straight_agg_preds * wt2(end) ... 55 | + left_lane_passive * wt3(end) ... 56 | + left_lane_aggressive * wt4(end); 57 | trail_begin = i-trail_length; 58 | if trail_begin < 1 59 | trail_begin = 1; 60 | end 61 | front_vehicle_positions = [filter_traj(trail_begin:i).front_car_position]; 62 | left_lane_vehicle_positions = [filter_traj(trail_begin:i).leftLaneVehPosn]; 63 | tiledlayout(3,1); 64 | nexttile; 65 | 66 | plot(meas(1, 1:i), meas(2, 1:i), ... 67 | estim(1, 1:i), estim(4, 1:i), ... 68 | wted_pred(1, :), wted_pred(4,:), ... 69 | front_vehicle_positions, right_lane_center(trail_begin:i), ... 70 | left_lane_vehicle_positions, left_lane_center(trail_begin:i), ... 71 | 'Linewidth', 1.5); 72 | 73 | ylabel('y coordinate (m)'); 74 | xlabel('x coordinate (m)'); 75 | xlim([0 estim(1, end)]); 76 | ymin = min(meas(2,:))-2; 77 | ymax = max(meas(2,:))+2; 78 | ylim([ymin ymax]); 79 | legend('measurement', 'estimate', 'prediction', 'front vehicle', 'left lane vehicle'); 80 | legend('Location', 'eastoutside'); 81 | title('Participant trajectories X vs Y (m)'); 82 | grid on 83 | 84 | nexttile; 85 | plot(estim(1, 1:i), wt1, ... 86 | estim(1, 1:i), wt2, ... 87 | estim(1, 1:i), wt3, ... 88 | estim(1, 1:i), wt4, ... 89 | 'Linewidth', 1.5); 90 | grid on 91 | xlabel('x coordinate (m)'); 92 | xlim([0 estim(1, end)]); 93 | ylabel('probabilistic weights'); 94 | ylim([-0.1 1.1]); 95 | legend('straight passive', 'straight aggressive', 'left LC long' ... 96 | , 'left LC short'); 97 | legend('Location', 'eastoutside'); 98 | title('Behavior weights (probability vs time)'); 99 | 100 | nexttile 101 | 102 | plot(estim(1, 1:i), driver_wts(1,1:i), ... 103 | estim(1, 1:i), driver_wts(2,1:i), ... 104 | 'Linewidth', 1.5); 105 | grid on 106 | xlabel('x coordinate (m)'); 107 | xlim([0 estim(1, end)]); 108 | ylabel('probabilistic weights'); 109 | ylim([-0.1 1.1]); 110 | legend('aggressive driver', 'passive driver'); 111 | legend('Location', 'eastoutside'); 112 | title('Driver type weights vs time'); 113 | 114 | pause(0.2); 115 | end 116 | end 117 | --------------------------------------------------------------------------------