├── .gitignore ├── README.md ├── matlab_simulation ├── 01-examples_lecture │ ├── mr-2-gaussianMath │ │ └── GaussianMath.m │ ├── mr-2-leastSquares │ │ └── LeastSquares.m │ ├── mr-2-randomNoise │ │ └── randomnoise.m │ ├── mr-2-transforms │ │ ├── Transforms2D.m │ │ └── Transforms3D.m │ ├── mr-3-continuous2Discrete │ │ └── cont2disc.m │ ├── mr-3-motionModels │ │ ├── example_AUV_USV_Husky.m │ │ ├── example_bicycle.m │ │ └── example_dubins_disturbance.m │ ├── mr-4-gdop │ │ └── gdop.m │ ├── mr-4-gyroCovariance │ │ ├── gyroCovEstimation.m │ │ └── plot_gyro.m │ ├── mr-4-scanner │ │ └── example_scanner.m │ ├── mr-5-bayesfilter │ │ ├── bayes_filter_example.m │ │ └── bayes_filter_example_simple.m │ ├── mr-5-extendedkf │ │ └── ekf_example.m │ ├── mr-5-kalmamfilter │ │ ├── kf_example1.m │ │ ├── kf_example2.m │ │ └── kf_example3.m │ ├── mr-5-particlefilter │ │ ├── flyover_filter_comparison.m │ │ ├── importance.m │ │ ├── nonlinear.m │ │ ├── particle_filter.m │ │ └── resampling.m │ ├── mr-5-unscentedkf │ │ └── ukf.m │ ├── mr-6-ekflocaliztion │ │ └── EKF_LOCALIZATION.m │ ├── mr-6-ekfslam │ │ └── ekf_slam.m │ ├── mr-6-occupancyGrid │ │ ├── occGridMapping.m │ │ └── occGridMappingWCollAv.m │ ├── mr-6-particlelocalization │ │ └── particle_localization.m │ ├── mr-6-scanRegistration │ │ ├── scanRegistrationDemo.m │ │ ├── scanregistration.m │ │ └── scanregistration2Dtry.m │ ├── mr-8-NLP │ │ ├── nonlinearplan.m │ │ └── nonlinearrhc.m │ ├── mr-8-PRM │ │ ├── batch_prm.m │ │ ├── direct_prm.m │ │ └── online_prm.m │ ├── mr-8-RRT │ │ ├── direct_rrt.m │ │ └── rrt.m │ └── mr-8-graphsearch │ │ ├── exampleOmniVoronoi.m │ │ ├── example_search_graph.m │ │ └── wavefront_example.m ├── 02-examples_practice │ ├── gallerypfield │ │ └── potentialField.m │ ├── gallerywavefront │ │ └── gallerywavefront.m │ ├── maze_sampling │ │ ├── PRM_maze_solver.m │ │ ├── deterministic_maze_solver.m │ │ └── integer_PRM_maze_solver.m │ └── spacebot_kalman_filter │ │ └── spacebot.m ├── 03-motion │ ├── AUV.m │ ├── Husky.m │ ├── USV.m │ ├── airplane_radar_linearized_motion_model.m │ ├── airplane_radar_motion_model.m │ ├── bicycle.m │ ├── dubins.m │ ├── motion_model.m │ ├── simpleLinearizedRobotMotionModel.m │ ├── simpleRobotMotionModel.m │ ├── twowheel.m │ ├── udlr_fullpath_motion.m │ └── udlr_motion.m ├── 04-sensor │ ├── airplane_radar_linearized_measurement_model.m │ ├── airplane_radar_measurement_model.m │ ├── bearing.m │ ├── bresenham.m │ ├── closestfeature.m │ ├── get2dpointmeasurement.m │ ├── get_sonar_range.m │ ├── getranges.m │ ├── inverse_scanner_bres.m │ ├── inverse_scanner_window.m │ ├── inversescanner.m │ ├── inversescannerbres.m │ ├── inview.m │ ├── measurement_model.m │ ├── range.m │ ├── simpleLinearizedRobotMeasurementModel.m │ └── simpleRobotMeasurementModel.m ├── 05-estimation │ ├── EKF.m │ ├── UKF │ │ ├── CompareEKF_UKF.m │ │ ├── CovarianceUpdate.m │ │ ├── CrossCovariance.m │ │ ├── ExtractMuSig.m │ │ ├── Ground.m │ │ ├── PlotData.m │ │ ├── SelectSigPoints.m │ │ ├── Update.m │ │ └── WeightedValues.m │ ├── bayes_filter.m │ ├── ekf.m │ ├── ekf_measurement_update.m │ ├── ekf_prediction_update.m │ ├── kalman_filter.m │ ├── multisensorekf.m │ ├── pf.m │ ├── pf_nonlinear.m │ ├── ukf_measurement_update.m │ └── ukf_prediction_update.m ├── 06-mapping │ ├── ekf_slam │ │ ├── ekfSLAMplot.m │ │ ├── range_bearing_meas_linearized_model.m │ │ ├── range_bearing_meas_linearized_model2.m │ │ ├── range_bearing_meas_model.m │ │ └── rearrangeMap.m │ ├── fastSLAM │ │ ├── addFeature.m │ │ ├── ekf_correct.m │ │ ├── featureSearch_fs.m │ │ ├── func_fastSLAM.m │ │ ├── func_fastSLAM2.m │ │ ├── measurementUpdate_fs.m │ │ ├── motionUpdate_fs.m │ │ ├── plot_fs.m │ │ ├── predict_features.m │ │ ├── raoBlackwell_fs.m │ │ ├── run_fastSLAM.m │ │ ├── sample_proposal.m │ │ └── staticMap.mat │ ├── icp.m │ ├── icp2try.m │ ├── logit.m │ ├── ogmap_update.m │ └── pf_localization.m ├── 07-control │ ├── car_trajectory.m │ ├── collision_pred.m │ └── run_lqr.m ├── 08-planning │ ├── nlpconstraints.m │ ├── nlpcost.m │ ├── shortest_wavefront_path.m │ ├── shortestpath_mr.m │ ├── trapezoidalDecomposition.m │ ├── voronoiDecomposition.m │ └── wavefront.m ├── 09-utilities │ ├── Trajectory_bazier.m │ ├── Trajectory_bezier.m │ ├── angleWrap.m │ ├── animation │ │ ├── Animation_GIF.m │ │ └── sample_animation.m │ ├── bresenham.m │ ├── error_ellipse.m │ ├── geographic │ │ ├── d2dms.m │ │ ├── dms2d.m │ │ ├── enu2wgslla.m │ │ ├── enu2wgsxyz.m │ │ ├── rotenu2xyz.m │ │ ├── rotxyz2enu.m │ │ ├── wgslla2enu.m │ │ ├── wgslla2xyz.m │ │ ├── wgsxyz2enu.m │ │ └── wgsxyz2lla.m │ ├── geometry │ │ ├── AffineIntersect.m │ │ ├── CheckCollision.m │ │ ├── CheckCollisionMaze.m │ │ ├── CheckCollisionSquare.m │ │ ├── CheckCollisionSquare2.m │ │ ├── Dist2Poly.m │ │ ├── EdgeCollision.m │ │ ├── EdgePtsToVec.m │ │ ├── SpinCalc.m │ │ ├── distToEdge.m │ │ ├── distanceToLineSegment.m │ │ ├── image_to_binary_map.m │ │ ├── minDistToEdges.m │ │ ├── polygonsOverlap.m │ │ ├── rot.m │ │ └── rot2D.m │ ├── linked_List.m │ ├── sampling │ │ ├── is_resampling.m │ │ ├── is_sampling.m │ │ └── is_weighting.m │ └── visualization │ │ ├── circle.m │ │ ├── drawbot.m │ │ ├── drawbox.m │ │ ├── drawcar.m │ │ ├── drawquadrotor.m │ │ ├── error_ellipse.m │ │ ├── flyover │ │ ├── plot_ellipse.m │ │ ├── plot_errors.m │ │ └── plot_flyover.m │ │ ├── importance_sampling │ │ ├── plot_distribution_importance.m │ │ ├── plot_initial_sample.m │ │ └── plot_sample_weights.m │ │ ├── nonlinear │ │ ├── ekf_approximation.m │ │ ├── linear_transform.m │ │ ├── nonlinear_transform.m │ │ ├── plot_linear_transform.m │ │ ├── plot_nonlinear_transform.m │ │ ├── plot_resulting_distributions.m │ │ └── unscented_approximation.m │ │ ├── particle_filter │ │ ├── plot_first_time_step.m │ │ ├── plot_measurement.m │ │ ├── plot_prediction.m │ │ ├── plot_prior_belief.m │ │ └── plot_trajectory.m │ │ ├── plot_cell_map.m │ │ ├── plot_inverse_mm.m │ │ ├── plot_localization_results.m │ │ ├── plot_localization_state.m │ │ ├── plot_occupancy_grid.m │ │ ├── plot_robot_path.m │ │ └── quad_starmac.mat ├── 10-environments │ ├── create_a_map.m │ ├── load_cell_map.m │ ├── makegallerymap.m │ ├── maze.m │ ├── plotEnvironment.m │ └── polygonal_world.m ├── 11-datasets │ ├── E5_Third_Floor.png │ ├── funnymap.jpg │ └── gyroData.mat ├── 12-test │ ├── runAllLectureExamples.m │ └── test_drawquadrotor.m ├── MobileRoboticsSetup.m └── code v1.0 │ ├── ME597-10-CollisionAvoidance │ ├── constraints.m │ ├── cost.m │ └── nonlinearrhc.m │ ├── ME597-10-FlyoverParticle │ └── flyover.m │ ├── ME597-10-GalleryPField │ ├── gallerypfield │ │ ├── distToEdge.m │ │ ├── gallery.m │ │ ├── minDistToEdges.m │ │ └── potentialField.m │ └── gallerywavefront │ │ ├── gallery.m │ │ ├── gallerywavefront.m │ │ └── wavefront.m │ ├── ME597-10-RansacSLAM │ ├── EKF_meas.m │ ├── ekf_ransac_slam.m │ ├── error_ellipse.m │ └── inview.m │ ├── ME597-10-VerticalMapping │ ├── getranges.m │ ├── groundmapping.m │ └── inversescanner.m │ ├── ME597-5-BayesFilter │ └── bayesfilter.m │ ├── ME597-5-Ellipse │ └── ellipse.m │ ├── ME597-5-ExtendedKalmanFilter │ ├── EKF.m │ ├── EKF_omnibot_example.m │ ├── MultiRateEKFGit.m │ ├── MultiRateEKFGit.m~ │ ├── Noise.m │ ├── error_ellipse.m │ ├── main.m │ ├── omniRobotFrame.m │ ├── omniRobotFrame.m~ │ ├── omnibot_linearize_motion_model.m │ ├── omnibot_linearize_sensor_model.m │ ├── omnibot_motion_model.m │ ├── omnibot_sensor_model.m │ ├── toRad.m │ └── worldFrame.m │ ├── ME597-5-KalmanFilter │ ├── error_ellipse.m │ ├── kalmanI.m │ ├── kalmanII.m │ └── kalmanIII.m │ ├── ME597-5-ParticleFilter │ ├── ekfukfparticle.m │ ├── ellipse.m │ ├── error_ellipse.m │ ├── expectation.m │ ├── importance.m │ ├── nonlinear.m │ ├── nonlinearmapping.m │ ├── normal.m │ ├── particle.m │ └── resampling.m │ ├── ME597-5-Unscented Kalman Filter │ ├── error_ellipse.m │ └── ukf.m │ ├── ME597-6-EKFLocalization │ ├── circle.m │ ├── ekf.m │ ├── ekf_localization_v2.m │ ├── error_ellipse.m │ ├── get2dpointmeasurement.m │ ├── inview.m │ └── multisensorekf.m │ ├── ME597-6-EKFSLAM │ ├── ekfSLAMplot.m │ ├── ekf_slam.m │ ├── error_ellipse.m │ ├── inview.m │ ├── range_bearing_meas_linearized_model.m │ ├── range_bearing_meas_model.m │ ├── rearrangeMap.m │ ├── two_wheel_motion_linearized_model.m │ └── two_wheel_motion_model.m │ ├── ME597-6-FastSLAM │ ├── fast_slam.m │ ├── inview.m │ └── newCode │ │ ├── ekf_fs.m │ │ ├── fastSLAM_func.m │ │ └── runFastSLAM.m │ ├── ME597-6-ParticleLocalization │ ├── closestfeature.m │ ├── inview.m │ ├── particle_localization.m │ └── particle_localization_new.m │ ├── ME597-6-RANSAC │ ├── inview.m │ └── ransac.m │ ├── ME597-6-ScanRegistration │ ├── bresenham.m │ ├── demo.m │ ├── funnymap.jpg │ ├── getranges.m │ ├── icp.m │ ├── icp2try.m │ ├── inversescannerbres.m │ ├── scanregistration.m │ └── scanregistration2Dtry.m │ ├── ME597-7-LQR_LQT │ ├── pitchlqr.m │ ├── pitchlqt.m │ └── run_lqr.m │ ├── ME597-7-NonlinearSteering │ ├── cartraj.m │ └── drawbox.m │ ├── ME597-7-Trajectories │ ├── corner.m │ ├── drawcar.m │ ├── input_check.m │ ├── lane_change.m │ ├── motion_model.m │ ├── nudges.m │ ├── obstacle.m │ ├── plot_figure.m │ ├── robottraj.m │ ├── spiral.m │ ├── swerves.m │ └── user_define_motion_model.m │ ├── ME597-8-LP │ └── linearplan.m │ ├── ME597-8-MILP │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── EdgeCollision.m │ ├── EdgePtsToVec.m │ ├── MILPplan.m │ ├── lp_maker.m │ ├── lpsolve55.dll │ ├── mxlpsolve.mexw64 │ ├── plotEnvironment.m │ ├── polygonal_world.m │ └── polygonsOverlap.m │ ├── ME597-8-PRM │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── Dist2Poly.m │ ├── EdgeCollision.m │ ├── EdgePtsToVec.m │ ├── plotEnvironment.m │ ├── polygonal_world.m │ ├── polygonsOverlap.m │ ├── shortestpath.m │ └── trajrollout.m │ ├── ME597-8-PotentialFields │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── EdgeCollision.m │ ├── EdgePtsToVec.m │ ├── distToEdge.m │ ├── minDistToEdges.m │ ├── plotEnvironment.m │ ├── polygonal_world.m │ ├── polygonsOverlap.m │ └── potentialField.m │ ├── ME597-8-Trapezoid │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── EdgeCollision.m │ ├── EdgePtsToVec.m │ ├── Trapezoid.m │ ├── calculatePolyPoints.m │ ├── closeTraps.m │ ├── plotEnvironment.m │ ├── plotTrapezoids.m │ ├── polygonal_world.m │ ├── polygonsOverlap.m │ └── trapezoidalDecomposition.m │ ├── ME597-8-Visibility │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── EdgeCollision.m │ ├── EdgePtsToVec.m │ ├── Visibility.m │ ├── plotEnvironment.m │ ├── polygonal_world.m │ └── polygonsOverlap.m │ ├── ME597-8-Voronoi │ ├── IKOmniDirectionRobot.m │ ├── Voronoi.m │ ├── createVoronoiGraph.m │ ├── distanceClosestPointToLine.m │ ├── distanceTwoPoints.m │ ├── erasePath.m │ ├── getVelocityVectors.m │ └── outOfBounds.m │ └── ME597-8-Wavefront │ ├── AffineIntersect.m │ ├── CheckCollision.m │ ├── EdgeCollision.m │ ├── EdgePolyIntersect.m │ ├── EdgePtsToVec.m │ ├── distToEdge.m │ ├── minDistToEdges.m │ ├── plotEnvironment.m │ ├── polygonal_world.m │ ├── polygonsOverlap.m │ └── wavefront.m └── turtlebot └── turtlebot_example ├── CMakeLists.txt ├── launch ├── amcl.launch.xml ├── amcl_demo.launch ├── amcl_live_demo.launch ├── gmapping.launch.xml ├── gmapping_live_demo.launch ├── gmapping_sim_demo.launch ├── sample_world.world └── turtlebot_gazebo.launch ├── package.xml └── src └── turtlebot_example_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # matlab_simulation ignores 2 | *.avi 3 | *.mp4 4 | *.asv 5 | pathdef.m 6 | 7 | # misc 8 | *.*~ 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mobilerobotics 2 | Matlab and Robot code for MTE 544: Autonomous Mobile Robotics at the University of Waterloo 3 | 4 | This repository includes a Matlab simulation library and Turtlebot example code for the MTE 544 course. 5 | 6 | For the Matlab simulation library, run MobileRoboticsSetup.m to add all necessary subfolders to your Matlab path. 7 | 8 | For the Turtlebot example code, see the lab instructions posted on the internal Learn website for the course. 9 | 10 | 11 | * MATLAB Programming Style Guide [link](https://sites.google.com/site/matlabstyleguidelines/home) 12 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-2-gaussianMath/GaussianMath.m: -------------------------------------------------------------------------------- 1 | %% Gaussian Math 2 | % Test addition, multiplication and division of Gaussians to see if result 3 | % is still Gaussian, by sampling excessively and performing computations on 4 | % samples, then comparing histograms to Gaussian approximations of final 5 | % distribution. 6 | 7 | % Distribution samples 8 | n = 100000; 9 | x = randn(n,1); 10 | y = 2*randn(n,1); 11 | 12 | % Math 13 | z1 = x+y; 14 | z2 = x.*y; 15 | z3 = x./y; 16 | 17 | % Histogram fit 18 | 19 | figure(1); clf; hold on; 20 | histfit(z1); 21 | xlim([-10 10]) 22 | title('Addition of Gaussians N1(0,1) + N2(0,4)') 23 | 24 | figure(2); clf; hold on; 25 | histfit(z2); 26 | xlim([-10 10]) 27 | title('Multiplication of Gaussians N1(0,1) * N2(0,4)') 28 | 29 | figure(3); clf; hold on; 30 | histfit(z3,10000); 31 | xlim([-200 200]) 32 | title('Division of Gaussians N1(0,1) / N2(0,4)') 33 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-2-randomNoise/randomnoise.m: -------------------------------------------------------------------------------- 1 | %% Random Noise simulation 2 | % Generates Gaussian samples with specified mean and covariance, and 3 | % places error ellipses of different probabilities on top. 4 | 5 | clear; clc; 6 | 7 | mu = [0; 0]; 8 | Q = [1 -1; -1 2]; % Covariance 9 | n = length(Q(:,1)); % Size 10 | [QE, Qe] = eig(Q) % Eigenvectors and eigenvalues of Sigma 11 | 12 | % Create a second Gaussian that swaps eigenvalues (or eigenvectors) 13 | RE = QE; 14 | Re(1,1) = Qe(2,2); 15 | Re(2,2) = Qe(1,1); 16 | 17 | % Create sample sets to represent the Gaussian distributions 18 | S = 10000; 19 | for i = 1:S 20 | ra(:,i) = randn(n,1); % Generate normal samples 21 | q(:,i) = mu + QE*sqrt(Qe)*ra(:,i); % Convert to samples with mean mu and covariance Q 22 | r(:,i) = mu + RE*sqrt(Re)*ra(:,i); % mean mu and covariance R 23 | end 24 | 25 | % Plot original distribution and error ellipses 26 | figure(1); clf; hold on; 27 | plot(q(1,:), q(2,:),'g.') 28 | error_ellipse(Q,mu,0.75); 29 | error_ellipse(Q,mu,0.95); 30 | axis equal 31 | 32 | % Plot modified distribution and original error ellipses 33 | figure(2); clf; hold on; 34 | plot(r(1,:), r(2,:),'g.') 35 | mu = [0 0]; 36 | error_ellipse(Q,mu,0.75); 37 | error_ellipse(Q,mu,0.95); 38 | axis equal 39 | 40 | 41 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-3-continuous2Discrete/cont2disc.m: -------------------------------------------------------------------------------- 1 | % Continuous to discrete dynamics example 2 | % Compares the discretization approaches described in lecture for a linear 3 | % state space model of an ROV with drag 4 | 5 | % Discrete time step 6 | dt = 0.1; % coarse 7 | dtc = 0.0001; % fine 8 | 9 | % Continuous model (2-D) 10 | b = 1; 11 | m = 2; 12 | A = [ 0 1 0 0; 0 -b/m 0 0; 0 0 0 1; 0 0 0 -b/m]; 13 | B = [0 0 ;1/m 0; 0 0; 0 1/m]; 14 | C = eye(4); 15 | D = zeros(4,2); 16 | sysc=ss(A,B,C,D); 17 | 18 | % Discrete model (rough approximation) 19 | Ad = [ 1 dt 0 0; 0 1-b/m*dt 0 0; 0 0 1 dt; 0 0 0 1-b/m*dt]; 20 | Bd = [0 0 ;1/m*dt 0; 0 0; 0 1/m*dt]; 21 | Cd = eye(4); 22 | Dd = zeros(4,2); 23 | sysd1 = ss(Ad,Bd,Cd,Dd,dt); 24 | 25 | % True zoh discretization 26 | sysd2 = c2d(sysc,dt,'zoh'); 27 | 28 | % foh discretization 29 | sysd3 = c2d(sysc,dt,'foh'); 30 | 31 | % Tustins discretization 32 | sysd4 = c2d(sysc,dt,'tustin'); 33 | 34 | % Simulation Initializations 35 | t = 0:dt:10; % Coarse time vector 36 | u = [sin(t);cos(t)]; 37 | x0 = [0 0 0 0]'; 38 | 39 | tc = 0:dtc:10; % Fine time vector 40 | uc = [sin(tc);cos(tc)]; 41 | 42 | 43 | % Simulations - performed using lsim 44 | yc=lsim(sysc,uc,tc,x0); 45 | y1=lsim(sysd1,u,t,x0); 46 | y2=lsim(sysd2,u,t,x0); 47 | y3=lsim(sysd3,u,t,x0); 48 | y4=lsim(sysd4,u,t,x0); 49 | 50 | % Plot trajectoies 51 | figure(1);clf; hold on; 52 | plot(yc(:,3),yc(:,1),'b') 53 | plot(y1(:,3),y1(:,1),'r') 54 | plot(y2(:,3),y2(:,1),'g') 55 | plot(y3(:,3),y3(:,1),'m') 56 | plot(y4(:,3),y4(:,1),'c') 57 | axis equal 58 | legend('Continuous','Rough','zoh','foh','tustin'); 59 | xlabel('East') 60 | ylabel('North') 61 | title('State propagation using various discretizations') 62 | 63 | 64 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-3-motionModels/example_dubins_disturbance.m: -------------------------------------------------------------------------------- 1 | %% Two-wheeled robot motion model 2 | 3 | 4 | dt = 1; % Timestep 5 | x0 = [0 0 0.1]'; % Initial State 6 | v = 1; % Speed 7 | w = 0.1; % Heading rate of change 8 | 9 | % Noise Model (x,y pos and heading) 10 | Q = [0.001 0 0; 11 | 0 0.001 0; 12 | 0 0 0.05]; 13 | [QE, Qe] = eig (Q); 14 | 15 | % Noise Model (speed and heading) 16 | R = [0.001 0; 17 | 0 0.05]; 18 | [RE, Re] = eig (R); 19 | 20 | n=1000; % Samples 21 | 22 | 23 | % disturbance free motion 24 | x1 = dubins(x0,v,w,dt) 25 | 26 | for i=2:n 27 | % Disturbance-driven motion 28 | D = QE*sqrt(Qe)*randn(3,1); 29 | E = RE*sqrt(Re)*randn(2,1); 30 | % Dynamics 31 | x_lin(:,i) = dubins(x0, v, w, dt) + D; 32 | x_nl(:,i) = dubins(x0+[0;0;dt*E(2)], v+E(1), w+E(2), dt); 33 | end 34 | 35 | % Plot 36 | figure(1); clf; hold on; 37 | plot( x0(1), x0(2), 'bo', 'MarkerSize',20, 'LineWidth', 3) 38 | plot( x1(1), x1(2), 'bo', 'MarkerSize',20, 'LineWidth', 3) 39 | plot( [x0(1) x1(1)], [x0(2) x1(2)],'b') 40 | plot( x_lin(1,:),x_lin(2,:), 'm.', 'MarkerSize', 3) 41 | plot( x_nl(1,:),x_nl(2,:), 'c.', 'MarkerSize', 3) 42 | title('Motion Model Distribution for two-wheeled robot') 43 | xlabel('x (m)'); 44 | ylabel('y (m)'); 45 | axis equal -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-4-gdop/gdop.m: -------------------------------------------------------------------------------- 1 | %% GDOP example 2 | clear; clc; 3 | 4 | % Satellite positions in lla (deg, deg, mA) 5 | sats = [0 -80 20000000; 6 | 90 -20 20000000; 7 | 45 -140 20000000; 8 | 60 -60 22000000; 9 | 40 -80 21000000; 10 | 50 -40 19000000; 11 | 70 -100 21000000; 12 | 30 -120 23000000; 13 | 15 -100 20000000;] 14 | 15 | % Current receiver position 16 | cur = [43 -80 337]; 17 | 18 | % Number of available satellites 19 | num = 4; 20 | 21 | for i=1:num 22 | % Convert satellite position to ENU 23 | satsenu(i,:) = wgslla2enu(sats(i,1), sats(i,2), sats(i,3), cur(1), cur(2), cur(3)); 24 | % Find range to satellite from receiver 25 | r = sqrt(satsenu(i,1)^2+satsenu(i,2)^2+satsenu(i,3)^2); 26 | % Calculate A matrix 27 | A(i,:) = [(satsenu(i,1)-cur(1))/r (satsenu(i,2)-cur(2))/r (satsenu(i,3)-cur(3))/r 1]; 28 | end 29 | 30 | % Find Q matrix 31 | Q = inv(A'*A); 32 | % Calculate GDOP 33 | GDOP = sqrt(trace(Q)); 34 | 35 | % Plot results 36 | figure(1); clf; hold on; 37 | plot3(cur(1),cur(2),cur(3), 'bx', 'MarkerSize', 10, 'LineWidth', 2); 38 | for i=1:num 39 | plot3(satsenu(i,1),satsenu(i,2),satsenu(i,3), 'ro', 'MarkerSize', 10,'LineWidth', 2); 40 | end 41 | grid on; -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-4-gyroCovariance/plot_gyro.m: -------------------------------------------------------------------------------- 1 | %% Plot fucntion for Gyro Data Estimation. 2 | 3 | % Plot the Original data 4 | figure(1);clf; 5 | subplot(2,2,1); hold on;grid on; 6 | plot(time_Stamp(t_start:t_stop), G_data(t_start:t_stop,:)); 7 | axis([tmin tmax -60 60]);title('1.Original Data');xlabel('Time(s)');ylabel('Gyro Rate (deg/s)');legend('Gx','Gy','Gz') 8 | %Plot the Simulated Data with vehicle motions 9 | subplot(2,2,2); hold on;grid on; 10 | plot(tg(t_start:t_stop), G_sim(:,(t_start:t_stop))); 11 | axis([tmin tmax -60 60]);title('2.Simulated Data');xlabel('Time(s)');ylabel('Gyro Rate (deg/s)');legend('Gx_{Sim}','Gy_{Sim}','Gz_{Sim}'); 12 | % Plot the Filtered Data 13 | subplot(2,2,3); hold on;grid on; 14 | plot(time_Stamp(t_start:t_stop), G_filtered(t_start:t_stop,:)); 15 | axis([tmin tmax -60 60]);title('3.Filtered Data');xlabel('Time(s)');ylabel('Gyro Rate (deg/s)');legend('Gx_f','Gy_f','Gz_f') 16 | % Plot the Predicted Gyro Data 17 | subplot(2,2,4); hold on;grid on; 18 | plot(time_Stamp(t_start:t_stop), G_out(:,t_start:t_stop)); 19 | axis([tmin tmax -60 60]);title('4.Estimated Data');xlabel('Time(s)');ylabel('Gyro Rate (deg/s)');legend('Gx_{pred}','Gy_{pred}','Gz_{pred}'); 20 | 21 | % Plot the Error Distribution 22 | figure(2); clf; hold on 23 | subplot(3,1,1);bar(Xex,Nex);hold on;plot(xvec,Gex,'r','LineWidth',2); 24 | title('Error Distribution in X');xlabel('Error Value');ylabel('Histogram Count'); 25 | subplot(3,1,2);bar(Xey,Ney);hold on;plot(xvec,Gey,'r', 'LineWidth',2); 26 | title('Error Distribution in Y');xlabel('Error Value');ylabel('Histogram Count'); 27 | subplot(3,1,3);bar(Xez,Nez);hold on;plot(xvec,Gez,'r', 'LineWidth',2); 28 | title('Error Distribution in Z');xlabel('Error Value');ylabel('Histogram Count'); 29 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-4-scanner/example_scanner.m: -------------------------------------------------------------------------------- 1 | % Laser scanner inverse measurement model plot 2 | clc; clear; 3 | 4 | M = 50; 5 | N = 60; 6 | m = 0.5*ones(M,N); %map 7 | 8 | alpha = 1; % Distance about measurement to fill in 9 | beta = 0.01; % Angle beyond which to exclude 10 | 11 | p_occ = 0.7; % Probability for occupied cells 12 | p_free = 0.3 % Probability for free cells 13 | 14 | % Robot location 15 | x = 25; 16 | y = 10; 17 | theta = pi/2; 18 | rmax = 80; 19 | 20 | % Measurements 21 | meas_phi = [-.4:0.01:.4]; % heading 22 | meas_r = 40*ones(size(meas_phi)); % range 23 | m = inversescanner(M,N,x,y,theta,meas_phi,meas_r,rmax,alpha,beta, p_occ, p_free); 24 | 25 | figure(2);clf;hold on; 26 | image(100*(1-m)); 27 | plot(y,x,'kx','MarkerSize',8,'LineWidth',2) 28 | % for i=1:length(meas_r) 29 | % plot( y+meas_r(i)*sin(meas_phi(i) + theta),x+meas_r(i)*cos(meas_phi(i)+ theta),'ko') 30 | % end 31 | colormap('gray') 32 | axis equal -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-5-particlefilter/importance.m: -------------------------------------------------------------------------------- 1 | %% Importance Sampling graphic 2 | clear; 3 | clc; 4 | 5 | %% Generate two interesting distributions 6 | % First distribution 7 | L = 5; 8 | mu = 0; % mean (mu) 9 | S = 0.5; % covariance (Sigma) 10 | x = [mu-L*sqrt(S):0.005:mu+L*sqrt(S)]; % x points 11 | gx = normpdf(x, mu, S); % p(x) 12 | gx = gx + 0.1; % Add uniform base probability 13 | gx = gx / sum(gx); % Normalize 14 | Gx = cumsum(gx); 15 | 16 | % Second distribution 17 | mu1 = -1; 18 | S1 = .25; 19 | fx1 = normpdf(x, mu1, S1); 20 | mu2 = -0; 21 | S2 = 0.5; 22 | fx2 = normpdf(x, mu2, S2); 23 | fx = fx1 + fx2 + 0.1; % form f(x) 24 | fx = fx / sum(fx); % normalize 25 | Fx = cumsum(fx); 26 | 27 | %% Importance sampling 28 | M = 20000; % number of particles 29 | [indP, xP, xPind] = is_sampling(M, Gx, x); 30 | [wx] = is_weighting(M, x, gx, fx, xP); 31 | [xPnew, xPindnew] = is_resampling(M, xP, wx); 32 | 33 | %% Plot 34 | plot_initial_sample(1, M, x, fx, gx, xP) 35 | plot_sample_weights(2, M, x, fx, gx, xP, wx) 36 | plot_distribution_importance(3, M, x, fx, gx, xPnew); 37 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-5-particlefilter/nonlinear.m: -------------------------------------------------------------------------------- 1 | %% Comparison of linear and nonlinear transformations 2 | % Original Gaussian distribution 3 | mu = 0; % mean (mu) 4 | S = 1; % covariance (Sigma) 5 | L = 5; 6 | x = [mu-L*sqrt(S):0.01:mu+L*sqrt(S)]; % x points 7 | px = normpdf(x,mu,S); % p(x) 8 | n = 5000000; % Number of samples to use 9 | xS = sqrt(S)*randn(n,1); % Samples of original distribution 10 | 11 | % Linear transformation and resulting Gaussian distribution 12 | [yL, muL, SL, pyL] = linear_transform(x, mu, S); 13 | 14 | % Nonlinear transformation 15 | [yN, YN, pyN, muN, yNG, pyNG] = nonlinear_transform(x, xS, n, L); 16 | 17 | % Linearized propagation of mean and covariance (a la EKF) 18 | [muLN, SLN, yLN, pyLN] = ekf_approximation(L, S, mu); 19 | 20 | % Unscented approximation to resulting distribution 21 | [X, Y, muU, yNU, pyNU] = unscented_approximation(L, S, mu); 22 | 23 | 24 | %% Display results 25 | plot_linear_transform(1, x, px, yL, pyL); 26 | plot_nonlinear_transform(2, x, px, yN, pyN, YN, pyNG, yNG, muN); 27 | plot_resulting_distributions(3, X, Y, yN, YN, pyN, yNG, pyNG, yLN, pyLN, yNU, pyNU, muN, muU, muLN); 28 | 29 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-5-particlefilter/resampling.m: -------------------------------------------------------------------------------- 1 | % resampling parameters 2 | M = 200; 3 | X = rand(M, 2); 4 | W = [1:M] ./ M; 5 | 6 | for t = 1:200 7 | % particle filter estimation 8 | for m = 1:M 9 | seed = rand(1); 10 | X(m, :, t + 1) = X(find(W > seed, 1), :, t); 11 | end 12 | end 13 | 14 | 15 | % plot resampling of particles 16 | t_range = [1, 2, 5, 10, 20, 30, 50, 70, 200]; 17 | figure(1); 18 | clf; 19 | 20 | for plot_index = 1:length(t_range) 21 | t = t_range(plot_index); 22 | 23 | subplot(3, 3, plot_index); 24 | plot(X(:, 1, t), X(:, 2, t),'ro'); 25 | axis([0 1 0 1]); 26 | xlabel(['t = ' num2str(t)]); 27 | end 28 | -------------------------------------------------------------------------------- /matlab_simulation/01-examples_lecture/mr-8-graphsearch/wavefront_example.m: -------------------------------------------------------------------------------- 1 | %test script 2 | close all 3 | testimage = imread('E5_Third_Floor.png'); 4 | testimage = image_to_binary_map(testimage); 5 | 6 | startpos = [103, 480]; 7 | endpos = [110, 400]; 8 | 9 | [wavefrontmap, path] = wavefront(testimage, startpos, endpos); 10 | 11 | % you can just run this function without rebuilding the wavefront map if the 12 | % goal location (or end point) does not change. 13 | 14 | path = shortest_wavefront_path(wavefrontmap, startpos); 15 | imagesc(wavefrontmap); 16 | hold on 17 | plot(path(:,1), path(:,2), '-r'); 18 | % in imagesc the x axis is up down and the y runs left right, so to plot you 19 | % need to reverse the ordering of the x and y of the start and end positions 20 | plot(startpos(2), startpos(1), 'gx', 'markersize', 15); 21 | plot(endpos(2), endpos(1), 'ro', 'markersize', 15); 22 | colorbar; 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/AUV.m: -------------------------------------------------------------------------------- 1 | function xcur = AUV(xprev,Tx, Ty, Tz, m,dt) 2 | %% AUV motion model with 3 thrusters and constant attitude. This function takes in previous state 3 | % xprev = [x,y,z,vx,vy,vz]' thrust values in Newtons, Tx,Ty,Tz, mass, m, 4 | % as well as the timestep dt. It returns the new robot pose. 5 | 6 | %Rough drag calculation, assuming a smooth sphere with radius 0.25m 7 | %Coeffecient of drag ~0.47, area=0.2m^2 8 | 9 | F_drag_x=-0.5*1000*0.47*0.2*xprev(4)^2; 10 | F_drag_y=-0.5*1000*0.47*0.2*xprev(5)^2; 11 | F_drag_z=-0.5*1000*0.47*0.2*xprev(6)^2; 12 | 13 | %change the direction of drag to be against direction of travel 14 | if xprev(4)<0 15 | F_drag_x=-F_drag_x; 16 | end 17 | 18 | if xprev(5)<0 19 | F_drag_y=-F_drag_y; 20 | end 21 | 22 | if xprev(6)<0 23 | F_drag_z=-F_drag_z; 24 | end 25 | 26 | %Update velocites 27 | vx=xprev(4)+ (Tx/m+F_drag_x/m)*dt; 28 | vy=xprev(5)+ (Ty/m+F_drag_y/m)*dt; 29 | vz=xprev(6)+ (Tz/m+F_drag_z/m)*dt; 30 | 31 | % Motion increment in the body frame, where v=(T/m)*dt 32 | dx_b = dt*[vx vy vz]'; 33 | 34 | %No rotation required 35 | 36 | % Robot state update in inertial frame 37 | xcur = [xprev(1:3) + dx_b;vx;vy;vz]; 38 | 39 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/Husky.m: -------------------------------------------------------------------------------- 1 | function xcur = Husky(xprev,omegal, omegar, dt) 2 | %% Motion model for Clearpath Robotics Husky UGV 3 | % xprev = [x,y,theta] Both wheels on each side for Husky are joined through 4 | % a drive belt therefore, both front and rear wheels on on the same side 5 | % will have identical rotational velocity. Let these angular velocites be 6 | % omegal and omegar. Husky's wheel radius is 0.17775m, and distance from 7 | % the wheels to the cog is 0.2854m. The timestep is dt. This function 8 | % returns the new robot pose. 9 | 10 | r = 0.1775; 11 | l = 0.2854; 12 | 13 | % Motion increment from applied forces -- left force (omegal) rotates the 14 | % body clockwise 15 | dx_b = dt*[r*omegal+ r*omegar 0 (-r*omegal*l+r*omegar*l)]'; 16 | 17 | % Rotation matrix for conversion from inertial frame to the robot frame. 18 | R = rot(xprev(3),3); 19 | 20 | % Robot state update in inertial frame -- use the transpose of R 21 | xcur = xprev + R'*dx_b; 22 | 23 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/USV.m: -------------------------------------------------------------------------------- 1 | function xcur = USV(xprev,Tl, Tr, m, ll, lr, dt) 2 | %% Motion model for a 2 thruster USV, Such as Clearpath Robotics Heron Platform 3 | % xprev = [x,y,theta,vx,vy,vtheta]'right thrust and left thrust are denoted 4 | % as Tl and Tr, mass of m, perpendicular distance from thruster to cog is 5 | % ll and lr for right thruster and left thruster respectivly as well as 6 | % the timestep dt. It returns the new robot pose. 7 | 8 | % Very rough drag calculated for Clearpath's Heron, comment this out and 9 | % remove from velocity equations if using a different USV, or sub in your 10 | % own vehicles properties. Note drag for theta was not modeled, so 11 | % rotational speed can get very large 12 | % A~0.31364m^2, assuming drag coeffecient of ~0.7 13 | 14 | % F_drag = 1/2 rho C_d A v^2 15 | F_drag=-0.5*1000*0.7*0.31364*xprev(4)*abs(xprev(4)); 16 | 17 | 18 | %update velocities 19 | v_x = xprev(4) + ((Tl/m + Tr/m + F_drag/m)*dt); 20 | v_theta = xprev(6) + ((- Tl*ll + Tr*lr)/m)*dt; 21 | 22 | % Motion increment in the body frame 23 | dx_b = [v_x*dt 0 v_theta*dt]'; 24 | 25 | % Rotation matrix for conversion from inertial frame to the robot frame. 26 | R = rot(xprev(3),3); 27 | 28 | % Robot state update in inertial frame -- use the transpose of R 29 | xcur = [xprev(1:3) + R'*dx_b; v_x;0;v_theta]; 30 | 31 | 32 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/airplane_radar_linearized_motion_model.m: -------------------------------------------------------------------------------- 1 | function A = airplane_radar_linearized_motion_model(~, ~, dt) 2 | % Implements the linearized motion model on slide 112 of EstimationI 3 | % Airplane flies horizontally with a constant velocity 4 | % The model is already linear, so no inputs are needed 5 | % dt - optional timestep 6 | % A - the A matrix of the linearized model 7 | if ~exist('dt', 'var') 8 | dt = 0.1; 9 | end 10 | Ad = [ 1 dt 0 ; 0 1 0; 0 0 1]; 11 | A = Ad; 12 | end -------------------------------------------------------------------------------- /matlab_simulation/03-motion/airplane_radar_motion_model.m: -------------------------------------------------------------------------------- 1 | function mup = airplane_radar_motion_model(mu, ~, dt) 2 | % Implements the motion model on slide 112 of EstimationI 3 | % Airplane flies horizontally with a constant velocity 4 | % mu - current state 5 | % dt - optional timestep 6 | % mup - predicted next state 7 | if ~exist('dt', 'var') 8 | dt = 0.1; 9 | end 10 | Ad = [ 1 dt 0 ; 0 1 0; 0 0 1]; 11 | mup = Ad*mu; 12 | end -------------------------------------------------------------------------------- /matlab_simulation/03-motion/bicycle.m: -------------------------------------------------------------------------------- 1 | function xcur = bicycle(xprev,v,delta,l,dt) 2 | %% Bicycle robot model kinematics. This function takes in previous state 3 | % xprev = [x,y,theta]', speed v and steering angle delta, the distance from 4 | % the front wheel to rear, l, as well as the timestep dt. It returns the new 5 | % robot pose. 6 | 7 | 8 | % Motion increment in the body frame 9 | dx_b = dt*[v 0 v*tan(delta)/l]'; 10 | 11 | % Rotation matrix for conversion from inertial frame to the robot frame. 12 | R = rot(xprev(3),3); 13 | 14 | % Robot state update in inertial frame -- use transpose of R 15 | xcur = xprev + R'*dx_b; 16 | 17 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/dubins.m: -------------------------------------------------------------------------------- 1 | function xcur = dubins(xprev,v,omega,dt) 2 | %% Dubins robot model kinematics. This function takes in previous state 3 | % xprev = [x,y,theta]', speed v and angular rate omega commands as 4 | % well as the timestep dt. It returns the new robot pose. 5 | 6 | 7 | % Motion increment in the body frame 8 | dx_b = dt*[v 0 omega]'; 9 | 10 | % Rotation matrix for conversion from inertial frame to the robot frame. 11 | R = rot(xprev(3),3); 12 | 13 | % Robot state update in inertial frame -- use transpose of R 14 | xcur = xprev + R'*dx_b; 15 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/simpleLinearizedRobotMotionModel.m: -------------------------------------------------------------------------------- 1 | function [ A ] = simpleLinearizedRobotMotionModel( mu, input, dt ) 2 | % Outputs linearized motion model from Mapping I slide 15 3 | 4 | if ~exist('dt', 'var') 5 | dt = 0.1; 6 | end 7 | 8 | A = [ 1 0 -input(1)*sin(mu(3))*dt; 9 | 0 1 input(1)*cos(mu(3))*dt; 10 | 0 0 1]; 11 | 12 | end 13 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/simpleRobotMotionModel.m: -------------------------------------------------------------------------------- 1 | function [ mup ] = simpleRobotMotionModel( state, input, dt ) 2 | % Outputs motion model from Mapping I slide 10 3 | 4 | if ~exist('dt', 'var') 5 | dt = 0.1; 6 | end 7 | 8 | mup = [state(1) + input(1)*cos(state(3))*dt; 9 | state(2) + input(1)*sin(state(3))*dt; 10 | state(3) + input(2)*dt]; 11 | 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/twowheel.m: -------------------------------------------------------------------------------- 1 | function xcur = twowheel(xprev,phidl,phidr,r,l,dt) 2 | %% Two wheel robot model kinematics. This function takes in previous state 3 | % xprev = [x,y,theta]', left wheel and right wheel rotation rates phidl 4 | % and phidr, the wheel radius r and the distance from wheel to cg l, as 5 | % well as the timestep dt. It returns the new robot pose. 6 | 7 | 8 | % Motion increment in the body frame 9 | dx_b = dt*[r*(phidl + phidr)/2 0 r*(phidr - phidl)/(2*l)]'; 10 | 11 | % Rotation matrix for conversion from inertial frame to the robot frame. 12 | R = rot(xprev(3),3); 13 | 14 | % Robot state update in inertial frame -- use transpose of R 15 | xcur = xprev + R'*dx_b; 16 | 17 | -------------------------------------------------------------------------------- /matlab_simulation/03-motion/udlr_motion.m: -------------------------------------------------------------------------------- 1 | function [x] = udlr_motion(map, x, v) 2 | % This function takes in a 0-1 occupancy grid map and a robot state 3 | % (x y heading), and attempts to move forward one step. If blocked, the 4 | % robot turns 90 degrees to the right instead. 5 | 6 | [M,N] = size(map); 7 | move = round(x(1:2) + v.*[cos(x(3)); sin(x(3))]); 8 | 9 | % If the robot hits a wall or obstacle, change direction 10 | if ((move(1)>M || move(2)>N || move(1)<1 || move(2)<1) || (map(move(1), move(2)) == 1)) 11 | x(3) = x(3)+pi/2; 12 | else 13 | x(1:2) = move; 14 | end 15 | 16 | return -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/airplane_radar_linearized_measurement_model.m: -------------------------------------------------------------------------------- 1 | function H = airplane_radar_linearized_measurement_model(mup, ~) 2 | % Implements the measurement model on slide 113 of EstimationI 3 | % The radar returns the straight-line distance from a point on the ground 4 | % to the airplane position 5 | % mup - predicted state 6 | % H - linearized measurement matrix 7 | H = [(mup(1))/(sqrt(mup(1)^2 + mup(3)^2)) 0 (mup(3))/(sqrt(mup(1)^2 + mup(3)^2))]; 8 | end -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/airplane_radar_measurement_model.m: -------------------------------------------------------------------------------- 1 | function yp = airplane_radar_measurement_model(mup, ~) 2 | % Implements the measurement model on slide 113 of EstimationI 3 | % The radar returns the straight-line distance from a point on the ground 4 | % to the airplane position 5 | % mup - predicted state 6 | % yp - predicted measurement 7 | yp = sqrt(mup(1)^2 + mup(3)^2); 8 | end -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/bearing.m: -------------------------------------------------------------------------------- 1 | function b = bearing(mx, my, x1, x2, x3) 2 | b = mod(atan2(my - x2, mx - x1) - x3 + pi, 2 * pi) - pi; 3 | end -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/bresenham.m: -------------------------------------------------------------------------------- 1 | function [x y]=bresenham(x1,y1,x2,y2) 2 | 3 | %Matlab optmized version of Bresenham line algorithm. No loops. 4 | %Format: 5 | % [x y]=bham(x1,y1,x2,y2) 6 | % 7 | %Input: 8 | % (x1,y1): Start position 9 | % (x2,y2): End position 10 | % 11 | %Output: 12 | % x y: the line coordinates from (x1,y1) to (x2,y2) 13 | % 14 | %Usage example: 15 | % [x y]=bham(1,1, 10,-5); 16 | % plot(x,y,'or'); 17 | x1=round(x1); x2=round(x2); 18 | y1=round(y1); y2=round(y2); 19 | dx=abs(x2-x1); 20 | dy=abs(y2-y1); 21 | steep=abs(dy)>abs(dx); 22 | if steep t=dx;dx=dy;dy=t; end 23 | 24 | %The main algorithm goes here. 25 | if dy==0 26 | q=zeros(dx+1,1); 27 | else 28 | q=[0;diff(mod([floor(dx/2):-dy:-dy*dx+floor(dx/2)]',dx))>=0]; 29 | end 30 | 31 | %and ends here. 32 | 33 | if steep 34 | if y1<=y2 y=[y1:y2]'; else y=[y1:-1:y2]'; end 35 | if x1<=x2 x=x1+cumsum(q);else x=x1-cumsum(q); end 36 | else 37 | if x1<=x2 x=[x1:x2]'; else x=[x1:-1:x2]'; end 38 | if y1<=y2 y=y1+cumsum(q);else y=y1-cumsum(q); end 39 | end -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/closestfeature.m: -------------------------------------------------------------------------------- 1 | function [m, ind] = closestfeature(map, x) 2 | % find closest feature in a map based on distance 3 | ind = 0; 4 | mind = inf; 5 | 6 | for i = 1:length(map(:, 1)) 7 | d = sqrt( (map(i, 1) - x(1))^2 + (map(i, 2) - x(2))^2); 8 | if (d < mind) 9 | mind = d; 10 | ind = i; 11 | end 12 | end 13 | 14 | m = transpose(map(ind, :)); 15 | end 16 | -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/getranges.m: -------------------------------------------------------------------------------- 1 | function meas_r = getranges(map,X,meas_phi, rmax) 2 | % Generate range measurements for a laser scanner based on a map, vehicle 3 | % position and sensor parameters. 4 | % Rough implementation of ray tracing algorithm. 5 | 6 | % Initialization 7 | [M,N] = size(map); 8 | x = X(1); 9 | y = X(2); 10 | th = X(3); 11 | meas_r = rmax*ones(size(meas_phi')); 12 | 13 | % For each measurement bearing 14 | for i=1:length(meas_phi) 15 | % For each unit step along that bearing up to max range 16 | for r=1:rmax 17 | % Determine the coordinates of the cell 18 | xi = round(x+r*cos(th+meas_phi(i))); 19 | yi = round(y+r*sin(th+meas_phi(i))); 20 | % If not in the map, set measurement there and stop going further 21 | if (xi<=1||xi>=M||yi<=1||yi>=N) 22 | meas_r(i) = sqrt((x-xi)^2+(y-yi)^2); 23 | break; 24 | % If in the map but hitting an obstacle, set measurement range and 25 | % stop going further 26 | elseif (map(xi,yi)) 27 | meas_r(i) = sqrt((x-xi)^2+(y-yi)^2); 28 | break; 29 | end 30 | end 31 | end 32 | -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/inverse_scanner_bres.m: -------------------------------------------------------------------------------- 1 | function [m] = inverse_scanner_bres(M, N, x, y, theta, r, rmax, p_occ, p_free) 2 | % Calculates the inverse measurement model for a laser scanner through 3 | % raytracing with Bresenham's algorithm, assigns low probability of object 4 | % on ray, high probability at end. Returns matrix of cell probabilities. 5 | % 6 | % Input: 7 | % M = Total Height of map 8 | % N = Total Width of map 9 | % x = Robot x position 10 | % y = Robot y position 11 | % theta = Angle of sensor 12 | % r = Range to measured object 13 | % rmax = Max range of sensor 14 | % p_occ = Probability of an occupied cell 15 | % p_free = Probability of an unoccupied cell 16 | % Output: 17 | % m = Matrix representing the inverse measurement model 18 | 19 | if (nargin<9) 20 | p_occ = 0.7; 21 | p_free = 0.3; 22 | end 23 | 24 | % Bound the robot within the map dimensions 25 | x1 = max(1, min(M, round(x))); 26 | y1 = max(1, min(N, round(y))); 27 | 28 | % Calculate position of measured object (endpoint of ray) 29 | endpt = [x y] + r * [cos(theta) sin(theta)]; 30 | 31 | % Bound the endpoint within the map dimensions 32 | x2 = max(1, min(M, round(endpt(1)))); 33 | y2 = max(1, min(N, round(endpt(2)))); 34 | 35 | % Get coordinates of all cells traversed by laser ray 36 | [coordlist(:, 1), coordlist(:, 2)] = bresenham(x1, y1, x2, y2); 37 | 38 | % Assign probabilities 39 | m = [coordlist p_free * ones(length(coordlist(:, 1)), 1)]; 40 | if (r < rmax) 41 | m(end, 3) = p_occ; 42 | end 43 | -------------------------------------------------------------------------------- /matlab_simulation/04-sensor/inversescanner.m: -------------------------------------------------------------------------------- 1 | function [m] = inversescanner(M,N,x,y,theta,meas_phi,meas_r,rmax,alpha,beta,p_occ,p_free) 2 | % Calculates the inverse measurement model for a laser scanner based on 3 | % model in Probabilistic Robotics - note: slow but easy to understand 4 | % Identifies three regions, the first where no new information is 5 | % available, the second where objects are likely to exist and the third 6 | % where objects are unlikely to exist. Returns an occupancy grid map of 7 | % size MXN with p(occupied|measurements) for each cell. 8 | % M, N - size of occupancy grid map (MXN) 9 | % x,y,theta - position of robot in map ( assumes 1 unit grid cells) 10 | 11 | % TODO: Modify to take in grid cell size. 12 | 13 | % Initialize measurement model 14 | m = 0.5*ones(M, N); 15 | 16 | % Range finder inverse measurement model 17 | for i = 1:M 18 | for j = 1:N 19 | % Find range and bearing to the current cell 20 | r = sqrt((i-x)^2+(j-y)^2); 21 | phi = mod(atan2(j-y,i-x)-theta+pi,2*pi)-pi; 22 | 23 | % Find the applicable range measurement 24 | [meas_cur,k] = min(abs(phi-meas_phi)); 25 | 26 | % If out of range, or behind range measurement, or outside of field 27 | % of view, no new information is available 28 | if (r > min(rmax, meas_r(k)+alpha/2) || (abs(phi-meas_phi(k))>beta/2)) 29 | m(i,j) = 0.5; 30 | 31 | % If the range measurement was in this cell, likely to be an object 32 | elseif ((meas_r(k)< rmax) && (abs(r-meas_r(k)) seed, 1)); 14 | end 15 | 16 | muParticle = mean(X); 17 | SParticle = var(X); 18 | end 19 | 20 | -------------------------------------------------------------------------------- /matlab_simulation/05-estimation/pf_nonlinear.m: -------------------------------------------------------------------------------- 1 | function [muParticle, SParticle, X] = pf_nonlinear(t, I, Ad, X, R, Q, y) 2 | [RE, Re] = eig(R); 3 | n = length(Ad(1,:)); 4 | 5 | % sample 6 | for i = 1:I 7 | ep = RE * sqrt(Re) * randn(n, 1); 8 | Xp(:, i) = Ad * X(:, i) + ep; 9 | w(i) = max(0.00001, normpdf(y(:, t), sqrt(Xp(1, i)^2 + Xp(3, i)^2), sqrt(Q))); 10 | end 11 | 12 | % re-sample 13 | W = cumsum(w); 14 | for j = 1:I 15 | seed = W(end) * rand(1); 16 | X(:,j) = Xp(:, find(W > seed, 1)); 17 | end 18 | muParticle = mean(X'); 19 | SParticle = cov(X'); 20 | end 21 | 22 | -------------------------------------------------------------------------------- /matlab_simulation/05-estimation/ukf_measurement_update.m: -------------------------------------------------------------------------------- 1 | function [K_u, mu_u, S_u] = ukf_measurement_update(t, Ad, Q, lambda, mup_u, Sp_u, x_sp, y, w_m, w_c) 2 | n = length(Ad(1,:)); 3 | m = length(Q(:,1)); 4 | 5 | nSp_u = sqrtm((n + lambda) * Sp_u); 6 | xp_sm(:, 1) = mup_u; 7 | y_sm(:, 1) = sqrt(xp_sm(1, 1)^2 + xp_sm(3, 1)^2); 8 | 9 | for i=1:n 10 | % Sigma points prior to measurement 11 | xp_sm(:, i + 1) = mup_u + transpose(nSp_u(i, :)); 12 | xp_sm(:, n + i + 1) = mup_u - transpose(nSp_u(i, :)); 13 | 14 | % Measurement model applied to sigma points 15 | y_sm(:, i + 1) = sqrt(xp_sm(1, i + 1)^2 + xp_sm(3, i + 1)^2); 16 | y_sm(:, n + i + 1) = sqrt(xp_sm(1, n + i + 1)^2 + xp_sm(3, n + i + 1)^2); 17 | end 18 | 19 | % Find measurement sigma point mean and covariance 20 | y_u = zeros(m, 1); 21 | for i = 1:(2 * n + 1) 22 | y_u = y_u + w_m(i) * y_sm(:, i); 23 | end 24 | Sy_u = zeros(m); 25 | for i = 1:(2 * n + 1) 26 | Sy_u = Sy_u + w_c(i) * ((y_sm(:, i) - y_u) * (y_sm(:, i) - y_u)'); 27 | end 28 | Sy_u = Sy_u + Q; 29 | 30 | % Find cross covariance between x and y sigma points 31 | Sxy_u = zeros(n,m); 32 | for i=1:(2 * n + 1) 33 | Sxy_u = Sxy_u + w_c(i) * ((x_sp(:, i) - mup_u) * (y_sm(:, i) - y_u)'); 34 | end 35 | 36 | % Perform measurement update 37 | K_u = Sxy_u * inv(Sy_u); 38 | mu_u = mup_u + K_u * (y(:,t)-y_u); 39 | S_u = Sp_u - K_u * Sy_u * K_u'; 40 | end 41 | 42 | -------------------------------------------------------------------------------- /matlab_simulation/05-estimation/ukf_prediction_update.m: -------------------------------------------------------------------------------- 1 | function [mup_u, Sp_u, x_sp, xp_sp] = ukf_prediction_update(Ad, S_u, mu_u, R, lambda, w_m, w_c) 2 | n = length(Ad(1,:)); 3 | 4 | % Prediction update 5 | nS_u = sqrtm((n + lambda) * S_u); 6 | xp_sp(:, 1) = mu_u; 7 | x_sp(:, 1) = Ad * xp_sp(:, 1); 8 | 9 | for i = 1:n 10 | % Sigma points prior to propagation 11 | xp_sp(:, i + 1) = mu_u + nS_u(:, i); 12 | xp_sp(:, n + i + 1) = mu_u - nS_u(:, i); 13 | 14 | % Sigma points after propagation 15 | x_sp(:, i + 1) = Ad * xp_sp(:, i + 1); 16 | x_sp(:, n + i + 1) = Ad * xp_sp(:, n + i + 1); 17 | end 18 | 19 | % calculate mean 20 | mup_u = zeros(n, 1); 21 | for i = 1:2 * n+1 22 | mup_u = mup_u + w_m(i) * x_sp(:, i); 23 | end 24 | 25 | % calculate covariance 26 | Sp_u = zeros(n); 27 | for i = 1:(2 * n + 1) 28 | Sp_u = Sp_u + w_c(i) * ((x_sp(:, i) - mup_u) * transpose((x_sp(:, i) - mup_u))); 29 | end 30 | Sp_u = Sp_u + R; % add motion variance as well 31 | end 32 | 33 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/ekf_slam/ekfSLAMplot.m: -------------------------------------------------------------------------------- 1 | function ekfSLAMplot(map,y,xr,mu_S,S,t,newfeature) 2 | % This function is used for plotting in ekfSLAM 3 | 4 | M=length(map(1,:)); % size of map 5 | N=length(xr(:,1))+2*M; % number of possible states 6 | 7 | subplot(1,2,1); hold on; 8 | plot(map(1,:),map(2,:),'go', 'MarkerSize',10,'LineWidth',2); 9 | plot(xr(1,1:t),xr(2,1:t), 'ro--') 10 | plot([xr(1,t) xr(1,t)+1*cos(xr(3,t))],[xr(2,t) xr(2,t)+1*sin(xr(3,t))], 'r-') 11 | plot(mu_S(1,1:t),mu_S(2,1:t), 'bx--') 12 | plot([mu_S(1,t) mu_S(1,t)+1*cos(mu_S(3,t))],[mu_S(2,t) mu_S(2,t)+1*sin(mu_S(3,t))], 'b-') 13 | mu_pos = [mu_S(1,t),mu_S(2,t)]; 14 | S_pos = [S(1,1) S(1,2); S(2,1) S(2,2)]; 15 | error_ellipse(S_pos,mu_pos,0.75); 16 | error_ellipse(S_pos,mu_pos,0.95); 17 | 18 | for i=1:M 19 | if (~newfeature(i)) 20 | fi = 2*(i-1)+1; 21 | fj = fi + 1; 22 | plot([xr(1,t) xr(1,t)+y(fi,t)*cos(y(fj,t)+xr(3,t))], [xr(2,t) xr(2,t)+y(fi,t)*sin(y(fj,t)+xr(3,t))], 'c'); 23 | plot(mu_S(3+fi,t),mu_S(3+fj,t), 'gx') 24 | mu_pos = [mu_S(3+fi,t) mu_S(3+fj,t)]; 25 | S_pos = [S(3+fi,3+fi) S(3+fi,3+fj); S(3+fj,3+fi) S(3+fj,3+fj)]; 26 | error_ellipse(S_pos,mu_pos,0.75); 27 | end 28 | end 29 | axis equal 30 | title('SLAM with Range & Bearing Measurements') 31 | 32 | % Plot Covariance 33 | subplot(1,2,2); 34 | image(10000*S); 35 | colormap('gray'); 36 | axis('square') 37 | axis([0.5,N+0.5,0.5,N+0.5]) 38 | title('Covariance matrix') 39 | end 40 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model.m: -------------------------------------------------------------------------------- 1 | function Ht = range_bearing_meas_linearized_model(mu,i) 2 | % linearized measurement model of range and bearing 3 | % can be found P.22 of Mapping II slides 4 | dx = mu(3+2*(i-1)+1)-mu(1); 5 | dy = mu(3+2*i)-mu(2); 6 | rp = sqrt((dx)^2+(dy)^2); 7 | 8 | N=length(mu); 9 | Fi = zeros(5,N); 10 | Fi(1:3,1:3) = eye(3); 11 | Fi(4:5,3+2*(i-1)+1:3+2*i) = eye(2); 12 | Ht = [ -dx/rp, -dy/rp, 0, dx/rp, dy/rp; 13 | dy/rp^2, -dx/rp^2, -1, -dy/rp^2, dx/rp^2]*Fi; 14 | end 15 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model2.m: -------------------------------------------------------------------------------- 1 | function Ht = range_bearing_meas_linearized_model2(mu,i) 2 | %% range_bearing_meas_linearized_model(mu,i) 3 | % linearized measurement model of range and bearing 4 | % can be found P.22 of Mapping II slides 5 | dx = mu(3+2*(i-1)+1)-mu(1); 6 | dy = mu(3+2*i)-mu(2); 7 | rp = sqrt((dx)^2+(dy)^2); 8 | 9 | N=length(mu); 10 | Fi = zeros(5,N); 11 | Fi(1:3,1:3) = eye(3); 12 | Fi(4:5,3+2*(i-1)+1:3+2*i) = eye(2); 13 | % Multiplying Ht by Fi maps Ht into the correct space 14 | Ht = [ -dx/rp, -dy/rp, 0, dx/rp, dy/rp; 15 | dy/rp^2, -dx/rp^2, -1, -dy/rp^2, dx/rp^2 ] * Fi; 16 | end 17 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_model.m: -------------------------------------------------------------------------------- 1 | function y = range_bearing_meas_model(xr,map) 2 | % measurement model of range and bearing 3 | % can be found P.12 of Mapping II slides 4 | y = [sqrt((map(1)-xr(1))^2 + (map(2)-xr(2))^2); 5 | atan2(map(2)-xr(2),map(1)-xr(1))-xr(3)]; 6 | end 7 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/ekf_slam/rearrangeMap.m: -------------------------------------------------------------------------------- 1 | function [map newfeature y flist]=rearrangeMap(map,newfeature,y,flist,i,count) 2 | % Function to reorganize the map, feature list, and measurements as new 3 | % feature is observed. 4 | % The newly discovered feature will be placed at the beginning of map, 5 | % newfeature, y (measurements), and flist 6 | % i: index for the newly observed feature in the feature list 7 | % count: total number of features that have been detected 8 | 9 | 10 | map=[map(:,i),map(:,1:(i-1)),map(:,(i+1):end)]; 11 | y=[y((2*i-1):2*i,:);y(1:2*(i-1),:);y((2*i+1):end,:)]; 12 | newfeature=[newfeature(i);newfeature(1:(i-1));newfeature((i+1):end)]; 13 | flist=[flist(i);flist(1:(i-1));flist((i+1):end)]; 14 | % mu_S=[mu_S(1:3,t);mu_S((3+2*i-1):(3+2*i),t)]; 15 | % mu_xr=mu_S(1:3,:); 16 | % mu_map=mu_S(4:end,:); 17 | % mu_map=[mu_map((2*i-1):2*i,:);mu_map(1:2*(i-1),:);mu_map((2*i+1):end,:)]; 18 | % mu_S=[mu_xr;mu_map] 19 | 20 | end 21 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/fastSLAM/addFeature.m: -------------------------------------------------------------------------------- 1 | %% A FUNCTION TO ADD UNOBSERVED FEATURES TO THE PARTICLES 2 | % INPUTS:(in order of call) 3 | % ------- 4 | % The prior mean of the feature 5 | % The observation that led to the feature's discovery 6 | % Uncertainty in that Measurement 7 | % Jacobian Matrix of the Observation with respect to the current particle 8 | % OUTPUTS:(in order of call) 9 | % -------- 10 | % Predicted Mean of newly observed feature 11 | % Predicted Covariance of the newly Observed Feature 12 | % % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | function [meanFeatP,covaFeatP]=addFeature(prior,obsFeat,H_feat,measUncertainty) 16 | meanFeatP(1,1) = prior(1,1)+obsFeat(1,1)*cos(obsFeat(2,1)+prior(3,1)); 17 | meanFeatP(2,1) = prior(2,1)+obsFeat(1,1)*sin(obsFeat(2,1)+prior(3,1)); 18 | covaFeatP= H_feat\measUncertainty*inv(H_feat)';%calculate cov 19 | end -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/fastSLAM/ekf_correct.m: -------------------------------------------------------------------------------- 1 | %% A function for the correction step of the EKF 2 | % NOTE: THE prediction step has already been called, Meas Jac Calculated 3 | % and new feature initialized, if at all. This is the next correction step 4 | % 5 | % INPUTS:(in order of call) 6 | % ------- 7 | % Mean of all observed Features 8 | % covariance of all Observed Features 9 | % Jacobian of the Measurement with respect to the features 10 | % Predicted Observation from the prediction step of EKF 11 | % Measurement Uncertainty 12 | % OUTPUTS:(in order of call) 13 | % -------- 14 | % Preicted mean of The feature 15 | % Predicted covariance of the feature 16 | % % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca 17 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 18 | 19 | function [meanFeatP,covaFeatP]=ekf_correct(muFeat,covFeat,obsFeat,H_feat,pred_obs,measUncertainty) 20 | I = obsFeat(:,1)-pred_obs; 21 | Q2 = H_feat*covFeat*H_feat' + measUncertainty; 22 | K = covFeat*H_feat'/Q2; 23 | meanFeatP = muFeat + K*I;%new mean 24 | covaFeatP = (eye(2)-K*H_feat)*covFeat;% New covariance 25 | end -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/fastSLAM/featureSearch_fs.m: -------------------------------------------------------------------------------- 1 | %% FUNC to search of features in Field of VIEW 2 | % INPUTS:(in order of call) 3 | % ------- 4 | % No of features 5 | % Map of the Environment 6 | % Robot's pose at the current time step 7 | % Maximum Range 8 | % Maximum FOV 9 | % OUTPUTS:(in order of call) 10 | % -------- 11 | % A vector containing all the measured indexs 12 | % % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | %% Feature Search 15 | function meas_ind=featureSearch_fs(noFeatures,map,robot_pose,rmax,thmax) 16 | meas_ind = []; 17 | for i=1:noFeatures 18 | if(inview(map(:,i),robot_pose,rmax,thmax)) 19 | meas_ind=[meas_ind,i];%concatenate the indices 20 | end 21 | end 22 | function yes = inview(f,x, rmax, thmax) 23 | % Checks for features currently in robot's view 24 | yes = 0; 25 | dx = f(1)-x(1); 26 | dy = f(2)-x(2); 27 | r = sqrt(dx^2+dy^2); 28 | th = mod(atan2(dy,dx)-x(3),2*pi); 29 | if (th > pi) 30 | th = th-2*pi; 31 | end 32 | if ((rseed,1); 28 | particles_new(particle).pose = particles_old(cur).pose; 29 | particles_new(particle).meanFeat = particles_old(cur).meanFeat; 30 | particles_new(particle).covFeat = particles_old(cur).covFeat; 31 | end 32 | for i=1:totalParticles 33 | particles_new(i).weight=particles_old(i).weight; 34 | end 35 | 36 | 37 | end -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/fastSLAM/sample_proposal.m: -------------------------------------------------------------------------------- 1 | %% Sampling of the Proposal Distribution based on current measurement 2 | %(unique to FAST SLAM2.0) 3 | % INPUTS:(in order of call) 4 | % ------- 5 | % Mean of the observed features 6 | % Covariance of the observed features 7 | % Robot's Current Pose 8 | % Observations at the current timestep 9 | % Measurement Noise 10 | % Motion Noise 11 | % OUTPUTS:(in order of call) 12 | % -------- 13 | % Estimated Mean of the Pose of the robot represented by the curr particle 14 | % Estimated Covariance of the pose of the robot of i-th particle 15 | % Jacobian of the measurement wrt to robot's pose 16 | % Jacobian of the measurement wrt to observed features 17 | % Predicted Observation 18 | % % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | 21 | function [mu_pose,cov_pose,H_pose,H_feat,pred_obs]=sample_proposal(muFeatOld,covFeatOld,rob_pose,act_obs,R,Q) 22 | dx = muFeatOld(1,1)-rob_pose(1,1); 23 | dy = muFeatOld(2,1)-rob_pose(2,1); 24 | rp = sqrt((dx)^2+(dy)^2); 25 | pred_obs=[rp; mod(atan2(dy,dx)-rob_pose(3,1)+pi,2*pi)-pi]; 26 | H_feat = [ dx/rp dy/rp; -dy/rp^2 dx/rp^2];% Calculate Jacobian wrt features 27 | H_pose = [-dx/rp -dy/rp 0; -dy/rp^2 -dx/rp^2 -1];%Jacobian wrt Pose 28 | Q_mat=(R+H_feat*covFeatOld*H_feat'); 29 | cov_pose=(H_pose'*((Q_mat)^-1)*H_pose+Q)^-1; 30 | mu_pose=cov_pose*H_pose'*(Q_mat^-1)*(act_obs-pred_obs)+rob_pose; 31 | end -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/fastSLAM/staticMap.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/06-mapping/fastSLAM/staticMap.mat -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/logit.m: -------------------------------------------------------------------------------- 1 | % Log odds graph 2 | 3 | p = 0:0.01:1; 4 | 5 | logitp = log(p)-log(1-p); 6 | 7 | plot(p,logitp) 8 | title('Log odds (logit)') 9 | xlabel('p') 10 | ylabel('logit(p)') 11 | 12 | -------------------------------------------------------------------------------- /matlab_simulation/06-mapping/pf_localization.m: -------------------------------------------------------------------------------- 1 | function [X] = pf_localization(t, dt, meas, n, D, R, Q, d, X, Xp, mf, y, u) 2 | [RE, Re] = eig(R); 3 | w = zeros(1, D); 4 | 5 | % sample 6 | for dd = 1:D 7 | e = RE * sqrt(Re) * randn(n, 1); 8 | Xp(:, dd) = [X(1, dd) + u(1, t) * cos(X(3, dd)) * dt; 9 | X(2, dd) + u(1, t) * sin(X(3, dd)) * dt; 10 | X(3, dd) + u(2, t) * dt] + e; 11 | switch(meas) 12 | case 1 % 1 - range 13 | r = range(mf(1, t), Xp(1, dd), mf(2, t), Xp(2, dd)); 14 | hXp = r + d; 15 | case 2 % 2 - bearing 16 | b = bearing(mf(1, t), mf(2, t), Xp(1, t), Xp(2, t), Xp(3, t)); 17 | hXp = b + d; 18 | case 3 % 3 - both 19 | r = range(mf(1, t), Xp(1, dd), mf(2, t), Xp(2, dd)); 20 | b = bearing(mf(1, t), mf(2, t), Xp(1, dd), Xp(2, dd), Xp(3, dd)); 21 | hXp = [r; b] + d; 22 | end 23 | w(dd) = max(1e-8, mvnpdf(y(:, t), hXp, Q)); 24 | end 25 | W = cumsum(w); 26 | 27 | % importance re-sample 28 | for dd = 1:D 29 | seed = max(W) * rand(1); 30 | X(:, dd) = Xp(:, find(W > seed, 1)); 31 | end 32 | end 33 | 34 | -------------------------------------------------------------------------------- /matlab_simulation/07-control/run_lqr.m: -------------------------------------------------------------------------------- 1 | function [x,u,Jx,Ju,P_S,K] = run_lqr(Ad,Bd,Q,R,t0,tf,dt,x0,ss) 2 | % Runs an lqr simulation from t0 to tf with the system defined by Ad, Bd,Q 3 | % and R 4 | 5 | % Costate setup 6 | T = t0:dt:tf; 7 | P = zeros(3,3,length(T)); 8 | P_S(:,:,length(T)) = Q; 9 | Pn = P_S(:,:,length(T)); 10 | 11 | % Solve for costate 12 | for t=length(T)-1:-1:1 13 | P = Q+Ad'*Pn*Ad - Ad'*Pn*Bd*inv(Bd'*Pn*Bd+R)*Bd'*Pn*Ad; 14 | P_S(:,:,t)=P; 15 | Pn=P; 16 | end 17 | 18 | % Setup storage structures 19 | x = zeros(3,length(T)); 20 | x(:,1) = x0'; 21 | u = zeros(1,length(T)-1); 22 | Jx = 0; 23 | Ju = 0; 24 | % Steady state comparison 25 | if (ss) 26 | Kss = dlqr(Ad,Bd,Q,R); 27 | end 28 | 29 | % Solve for control gain and simulate 30 | for t=1:length(T)-1 31 | K = inv(Bd'*P_S(:,:,t+1)*Bd + R)*Bd'*P_S(:,:,t+1)*Ad; 32 | u(:,t)=-K*x(:,t); 33 | if(ss) 34 | u(:,t)=-Kss*x(:,t); 35 | end 36 | x(:,t+1) = Ad*x(:,t)+Bd*u(:,t); 37 | 38 | Jx = Jx + 1/2*x(:,t)'*x(:,t); 39 | Ju = Ju + 1/2*u(:,t)'*u(:,t); 40 | end 41 | 42 | end -------------------------------------------------------------------------------- /matlab_simulation/08-planning/nlpconstraints.m: -------------------------------------------------------------------------------- 1 | function [C, Ceq] = constraints(x) 2 | 3 | global n N T dt obs withobs vd_cnst 4 | 5 | C = []; 6 | Ceq = zeros(n*(T-1),1); 7 | 8 | % Dynamics 9 | for i = 1:T-1 10 | xprev = x((N)*(i-1)+1:(N)*(i-1)+N); 11 | xcur = x((N)*(i)+1:(N)*(i)+N); 12 | Ceq(n*(i-1)+1:n*i, 1) = xcur(1:3)-xprev(1:3)-dt*[cos(xprev(3))*xprev(4); sin(xprev(3))*xprev(4); xprev(5)]; 13 | C = [C; x(4)-vd_cnst]; % Velocity constraint 14 | end 15 | 16 | % Environment 17 | C = []; 18 | if (withobs) 19 | nobs = length(obs(:,1)); 20 | for i = 1:T 21 | for j = 1:nobs 22 | C = [C; obs(j,3)^2-norm(obs(j,1:2)'-x(N*(i-1)+1:N*(i-1)+2))^2]; 23 | end 24 | end 25 | end 26 | 27 | % Velocity constraints 28 | ind_end=length(C); 29 | C(ind_end+1:ind_end+T,1)=x(4:N:end)-vd_cnst; 30 | -------------------------------------------------------------------------------- /matlab_simulation/08-planning/nlpcost.m: -------------------------------------------------------------------------------- 1 | function f = cost(x) 2 | 3 | global N T xd pF endonly 4 | 5 | f=0; 6 | for i=1:T 7 | xcur = x(N*(i-1)+1:N*i); 8 | 9 | % Position error cost 10 | if (~endonly) 11 | f = f + norm(xd(i,1:2) - xcur(1:2)'); 12 | end 13 | 14 | % Heading error cost 15 | %f = f + (xd(i,3)-xcur(3))^2; 16 | 17 | % Velocity control input cost 18 | f = f + 0.01*xcur(4)^2; 19 | 20 | % Turn rate control input cost 21 | f = f + 0.01*xcur(5)^2; 22 | end 23 | 24 | if (endonly) 25 | xcur = x(N*(T-1)+1:N*T); 26 | f = f + norm(pF(1:2) - xcur(1:2)'); 27 | end 28 | 29 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/Trajectory_bazier.m: -------------------------------------------------------------------------------- 1 | function xdT=Trajectory_bazier(xd_start,xd_midpoint,xd_end,dt,TTot) 2 | % This file generates random Bezier Curves by using 3 control points: start, middle, final ponit 3 | % Inputs of the function are arbitrary starting point, midle point, final point in x-y plane: [x,y] 4 | % dt is tme step, TTtot is number of iterations 5 | % The output of this function is [x,y,0] 6 | 7 | P=[xd_start;ones(size(xd_midpoint,1),1)*xd_start+xd_midpoint;xd_end]; % P is a set of points including start point, bezier control point and target. 8 | 9 | syms g 10 | Np = size(P, 1); % total number of points 11 | 12 | for i = 1:Np 13 | Bp(:,i) = nchoosek(Np,i-1).*(g.^(i-1)).*((1-g).^(Np-i+1)); % Bp is the Bernstein polynomial value 14 | end 15 | 16 | Bp1= (nchoosek(Np,Np).*(g.^Np)).'; 17 | 18 | Sp= simplify(Bp*P + Bp1*P(Np,:)); 19 | 20 | Bz_fun=matlabFunction(Sp); % matlabFunction is used as an alternative to subs() to improve the run time. 21 | 22 | xdT=[Bz_fun(([0:dt:(TTot)*dt]./(TTot*dt)).'),zeros(length([0:dt:(TTot*dt)]),1)]; 23 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/Trajectory_bezier.m: -------------------------------------------------------------------------------- 1 | function xdT=Trajectory_bezier(xd_start,xd_midpoint,xd_end,dt,TTot) 2 | % This file generates random Bezier Curves using a start point, middle points (control points),and a final ponit. 3 | % Inputs are arbitrary starting point and final point in x-y plane: [x,y] 2x1 4 | % and n middle points or control points in x-y plane, where n can be 1,2,3,...: [x,y] nx1 5 | % dt is tme step, TTtot is number of iterations 6 | % The output of this function is [x,y,0] 7 | 8 | P=[xd_start;ones(size(xd_midpoint,1),1)*xd_start+xd_midpoint;xd_end]; % P is a set of points including start point, bezier control point and target. 9 | 10 | syms g 11 | Np = size(P, 1); % total number of points 12 | 13 | for i = 1:Np 14 | Bp(:,i) = nchoosek(Np,i-1).*(g.^(i-1)).*((1-g).^(Np-i+1)); % Bp is the Bernstein polynomial value 15 | end 16 | 17 | Bp1= (nchoosek(Np,Np).*(g.^Np)).'; 18 | 19 | Sp= simplify(Bp*P + Bp1*P(Np,:)); 20 | 21 | Bz_fun=matlabFunction(Sp); % matlabFunction is used as an alternative to subs() to improve the run time. 22 | 23 | xdT=[Bz_fun(([0:dt:(TTot)*dt]./(TTot*dt)).'),zeros(length([0:dt:(TTot*dt)]),1)]; 24 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/angleWrap.m: -------------------------------------------------------------------------------- 1 | %% angleWrap 2 | % A function that receives an angle and binds it between [-pi, pi]. 3 | % 4 | % Input: 5 | % angle - Any arbitray angle measurement in radians 6 | % 7 | % Output: 8 | % angle - The same angle except numerically bound from -pi to pi 9 | function [wrapped_angle] = angleWrap(angle) 10 | 11 | wrapped_angle = mod(angle + pi, 2*pi) - pi; -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/animation/Animation_GIF.m: -------------------------------------------------------------------------------- 1 | function Animation_GIF( filename,i,h ) 2 | % This function generates animation file of the simulation and saves it to a .gif file. 3 | % h: is the figure handle 4 | % i: is the iteration index 5 | % filename: filename.gif 6 | 7 | getframe(h); 8 | 9 | 10 | % saving animation file in .gif file 11 | im = frame2im(getframe(1)); 12 | [imind,cm] = rgb2ind(im,256); 13 | if i == 1; 14 | imwrite(imind,cm,filename,'gif', 'Loopcount',inf); 15 | else 16 | imwrite(imind,cm,filename,'gif','WriteMode','append'); 17 | end 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/animation/sample_animation.m: -------------------------------------------------------------------------------- 1 | % A simple exmaple of using Animation_GIF function to genrate animation file and 2 | % save the result as .gif file 3 | 4 | clc; 5 | clear; 6 | syms t; 7 | filename='sample.gif'; 8 | 9 | dt=0.1; 10 | Time=0:dt:10; 11 | x=t; % random function 12 | y=sin(t); % random function 13 | 14 | for i=1:numel(Time) 15 | clf 16 | ti=Time(i); 17 | plot(subs(x,t,ti),subs(y,t,ti),'ro','MarkerEdgeColor','k',... 18 | 'MarkerFaceColor',[.49 1 .63],... 19 | 'MarkerSize',10); 20 | title('This result will be saved as an .gif animation file') 21 | axis([-2 10 -4 4 ]); 22 | grid on; 23 | Animation_GIF( filename,i,gcf ) 24 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/bresenham.m: -------------------------------------------------------------------------------- 1 | function [x y]=bresenham(x1,y1,x2,y2) 2 | 3 | %Matlab optmized version of Bresenham line algorithm. No loops. 4 | %Format: 5 | % [x y]=bham(x1,y1,x2,y2) 6 | % 7 | %Input: 8 | % (x1,y1): Start position 9 | % (x2,y2): End position 10 | % 11 | %Output: 12 | % x y: the line coordinates from (x1,y1) to (x2,y2) 13 | % 14 | %Usage example: 15 | % [x y]=bham(1,1, 10,-5); 16 | % plot(x,y,'or'); 17 | x1=round(x1); x2=round(x2); 18 | y1=round(y1); y2=round(y2); 19 | dx=abs(x2-x1); 20 | dy=abs(y2-y1); 21 | steep=abs(dy)>abs(dx); 22 | if steep t=dx;dx=dy;dy=t; end 23 | 24 | %The main algorithm goes here. 25 | if dy==0 26 | q=zeros(dx+1,1); 27 | else 28 | q=[0;diff(mod([floor(dx/2):-dy:-dy*dx+floor(dx/2)]',dx))>=0]; 29 | end 30 | 31 | %and ends here. 32 | 33 | if steep 34 | if y1<=y2 y=[y1:y2]'; else y=[y1:-1:y2]'; end 35 | if x1<=x2 x=x1+cumsum(q);else x=x1-cumsum(q); end 36 | else 37 | if x1<=x2 x=[x1:x2]'; else x=[x1:-1:x2]'; end 38 | if y1<=y2 y=y1+cumsum(q);else y=y1-cumsum(q); end 39 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/d2dms.m: -------------------------------------------------------------------------------- 1 | % Function [deg, min, sec] = d2dms(degrees) returns a 3 scalars 2 | % which are the scalar decimal values degrees represented 3 | % in degrees, arc-minutes, and arc-seconds 4 | % 5 | % Note: If degrees is negative, then deg, min, sec will all 6 | % be negative 7 | 8 | function [d, min, sec] = d2dms(deg) 9 | 10 | [m n] = size(deg); 11 | if m ~= 1 | n ~= 1 12 | warning('d2dms: input degrees not a scalar'); 13 | end 14 | 15 | d = fix(deg); 16 | min = fix(rem(deg*60,60)); 17 | sec = rem(deg*3600,60); 18 | 19 | return 20 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/dms2d.m: -------------------------------------------------------------------------------- 1 | % Function d = dms2d(deg, min, sec) creates a scalar output 2 | % value d which is decimal degrees based on the scalar 3 | % inputs deg, min, sec. 4 | % 5 | % Warning: Results may not be valid unless deg, min, and 6 | % sec are positive 7 | 8 | function d=dms2d(deg,min,sec) 9 | 10 | [m n] = size(deg); 11 | if m ~= 1 | n ~= 1 12 | warning('dms2d: deg input value not a scalar'); 13 | end 14 | 15 | [m n] = size(min); 16 | if m ~= 1 | n ~= 1 17 | warning('dms2d: min input value not a scalar'); 18 | end 19 | 20 | [m n] = size(sec); 21 | if m ~= 1 | n ~= 1 22 | warning('dms2d: sec input value not a scalar'); 23 | end 24 | 25 | d=deg+min/60+sec/3600; 26 | 27 | return; 28 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/enu2wgslla.m: -------------------------------------------------------------------------------- 1 | % Function [lat,lon,alt] = enu2wgslla(enu, reflat, reflon, refalt) 2 | % returns lat, lon, alt which represents the latitude (degrees), 3 | % longitude (degrees), and altitude above the ellipsoid (in 4 | % meters) of a point with East, North, Up position 5 | % enu (3 x 1 vector, units in meters) in an ENU coordinate system 6 | % located at latitude reflat (degrees), longitude reflon (degrees) 7 | % and altitude above the WGS84 ellipsoid refalt (meters) 8 | % 9 | % Note: Requires functions enu2wgsxyz.m and wgsxyz2lla.m to be 10 | % in the same directory 11 | 12 | function [lat, lon, alt] = wgslla2enu(enu, reflat, reflon, refalt) 13 | 14 | xyz = enu2wgsxyz(enu, reflat, reflon, refalt); 15 | [lat, lon, alt] = wgsxyz2lla(xyz); 16 | 17 | return -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/enu2wgsxyz.m: -------------------------------------------------------------------------------- 1 | % Function xyz = enu2wgsxyz(enu, reflat, reflon, refalt) returns 2 | % a 3 x 1 vector xyz which represents the WGS84 XYZ 3 | % coordinates (in meters) of a point with East, North, Up position 4 | % enu (3 x 1 vector, units in meters) in an ENU coordinate system 5 | % located at latitude reflat (degrees), longitude reflon (degrees) 6 | % and altitude above the WGS84 ellipsoid refalt (meters) 7 | % 8 | % Note: Requires functions wgslla2xyz.m and rot.m to be in the 9 | % same directory 10 | 11 | function xyz=enu2wgsxyz(enu, reflat, reflon, refalt) 12 | 13 | 14 | [m n] = size(enu); 15 | if m ~= 3 | n ~= 1 16 | error('enu input vector must be 3 x 1'); 17 | end 18 | 19 | % First, rotate the enu vector to xyz frame 20 | 21 | R1=rot(90+reflon, 3); 22 | R2=rot(90-reflat, 1); 23 | R=R2*R1; 24 | 25 | diffxyz=inv(R)*enu; 26 | 27 | % Then, calculate the xyz of reflat, reflon, refalt 28 | 29 | refxyz = wgslla2xyz(reflat, reflon, refalt); 30 | 31 | % Add diffxyz to refxyz 32 | 33 | xyz = diffxyz + refxyz; 34 | 35 | return; 36 | 37 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/rotenu2xyz.m: -------------------------------------------------------------------------------- 1 | %Function xyz=rotenu2xyz(enu (3x1), lat (degrees), lon(degrees)) 2 | %rotates a vector from enu reference frame at latitude lat and 3 | %longitude lon into a WGS84 xyz reference frame. 4 | % 5 | %Note: norm(xyz) and norm(enu) will remain identical 6 | % 7 | %Note: Requires the function rot.m to be in the same directory 8 | 9 | function xyz=rotenu2xyz(enu, lat, lon) 10 | 11 | R1=rot(90+lon, 3); 12 | R2=rot(90-lat, 1); 13 | 14 | R=R2*R1; 15 | xyz = inv(R)*enu; 16 | 17 | return; 18 | 19 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/rotxyz2enu.m: -------------------------------------------------------------------------------- 1 | %Function enu=rotxyz2enu(xyz (3x1), lat (degrees), lon(degrees)) 2 | %rotates a vector from the WGS84 xyz reference frame into an ENU 3 | %reference frame at latitude lat and longitude lon 4 | % 5 | %Note: norm(xyz) and norm(enu) will remain identical 6 | % 7 | %Note: Requires the function rot.m to be in the same directory 8 | 9 | function enu=rotxyz2enu(xyz, lat, lon) 10 | 11 | R1=rot(90+lon, 3); 12 | R2=rot(90-lat, 1); 13 | 14 | R=R2*R1; 15 | enu=R*xyz; 16 | 17 | return; 18 | 19 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/wgslla2enu.m: -------------------------------------------------------------------------------- 1 | % Function enu = wgslla2enu(lat,lon,alt,reflat,reflon,refalt) 2 | % returns a 3 x 1 vector enu representing the East, North, and Up 3 | % coordinates (in meters) of a point with coordinates represented 4 | % by latitude lat (degrees), longitude lon (degrees), and altitude 5 | % alt (meters above the ellipsoid) in an ENU coordinate system 6 | % located at latitude reflat (degrees), longitude reflon (degrees) 7 | % and altitude above the WGS84 ellipsoid refalt (meters) 8 | % 9 | % Note: requires the functions wgslla2xyz.m and wgsxyz2enu.m 10 | % to be in the same directory 11 | 12 | function enu = wgslla2enu(lat, lon, alt, reflat, reflon, refalt) 13 | 14 | xyz = wgslla2xyz(lat, lon, alt); 15 | enu = wgsxyz2enu(xyz, reflat, reflon, refalt); -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/wgslla2xyz.m: -------------------------------------------------------------------------------- 1 | %Function xyz = wgslla2xyz(lat, lon, alt) returns the 2 | %equivalent WGS84 XYZ coordinates (in meters) for a 3 | %given geodetic latitude "lat" (degrees), longitude "lon" 4 | %(degrees), and altitude above the WGS84 ellipsoid 5 | %in meters. Note: N latitude is positive, S latitude 6 | %is negative, E longitude is positive, W longitude is 7 | %negative. 8 | % 9 | %Ref: Decker, B. L., World Geodetic System 1984, 10 | %Defense Mapping Agency Aerospace Center. 11 | function xyz = wgslla2xyz(wlat, wlon, walt) 12 | 13 | 14 | A_EARTH = 6378137; 15 | flattening = 1/298.257223563; 16 | NAV_E2 = (2-flattening)*flattening; % also e^2 17 | deg2rad = pi/180; 18 | 19 | slat = sin(wlat*deg2rad); 20 | clat = cos(wlat*deg2rad); 21 | r_n = A_EARTH/sqrt(1 - NAV_E2*slat*slat); 22 | xyz = [ (r_n + walt)*clat*cos(wlon*deg2rad); 23 | (r_n + walt)*clat*sin(wlon*deg2rad); 24 | (r_n*(1 - NAV_E2) + walt)*slat ]; 25 | 26 | if ((wlat < -90.0) | (wlat > +90.0) |... 27 | (wlon < -180.0) | (wlon > +360.0)) 28 | error('WGS lat or WGS lon out of range'); 29 | end 30 | return 31 | 32 | 33 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geographic/wgsxyz2enu.m: -------------------------------------------------------------------------------- 1 | % Function enu = wgsxyz2enu(xyz, reflat, reflon, refalt) returns 2 | % a 3 x 1 vector enu which represents the East, North, and Up 3 | % coordinates (in meters) of a point with WGS84 xyz coordinates 4 | % xyz (3 x 1 vector, units in meters) in an ENU coordinate system 5 | % located at latitude reflat (degrees), longitude reflon (degrees) 6 | % and altitude above the WGS84 ellipsoid refalt (meters) 7 | % 8 | % Note: Requires functions wgslla2xyz.m and rot.m to be in the 9 | % same directory 10 | 11 | function enu=wgsxyz2enu(xyz, reflat, reflon, refalt) 12 | 13 | [m n] = size(xyz); 14 | if m ~= 3 | n ~= 1 15 | error('wgsxyz2enu: xyz input vector must be 3 x 1'); 16 | end 17 | 18 | [m n] = size(reflat); 19 | if m ~= 1 | n ~= 1 20 | error('wgsxyz2enu: reflat input vector must be scalar'); 21 | end 22 | 23 | [m n] = size(reflon); 24 | if m ~= 1 | n ~= 1 25 | error('wgsxyz2enu: reflon input vector must be scalar'); 26 | end 27 | 28 | [m n] = size(refalt); 29 | if m ~= 1 | n ~= 1 30 | error('wgsxyz2enu: refalt input vector must be scalar'); 31 | end 32 | 33 | % First, calculate the xyz of reflat, reflon, refalt 34 | 35 | refxyz = wgslla2xyz(reflat, reflon, refalt); 36 | 37 | % Difference xyz from reference point 38 | 39 | diffxyz = xyz - refxyz; 40 | 41 | % Now rotate the (often short) diffxyz vector to enu frame 42 | 43 | R1=rot(90+reflon, 3); 44 | R2=rot(90-reflat, 1); 45 | R=R2*R1; 46 | 47 | enu=R*diffxyz; 48 | 49 | return; 50 | 51 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/CheckCollisionMaze.m: -------------------------------------------------------------------------------- 1 | function [ collided ] = CheckCollisionMaze( s, f, map ) 2 | % parametrize the lines 3 | step = f - s; 4 | 5 | for i=1:size(map,1) 6 | e1 = [map(i,1) map(i,3)]; 7 | e2 = [map(i,2) map(i,4)]; 8 | estep = e2 - e1; 9 | 10 | q_num = (s(2)-e1(2))*step(1) - (s(1)-e1(1))*step(2); 11 | q_den = estep(2)*step(1) - estep(1)*step(2); 12 | 13 | if(q_den == 0) 14 | %disp('q_den zero'); 15 | continue; 16 | end 17 | 18 | q = q_num/q_den; 19 | 20 | if(q < 0 || q > 1) 21 | %disp('q not collided'); 22 | continue; 23 | end 24 | 25 | t_num = (s(2)-e1(2))*estep(1) - (s(1)-e1(1))*estep(2); 26 | t_den = q_den; 27 | %t_num = e1(1) + q*estep(1) - s(1); 28 | t = t_num / t_den; 29 | 30 | if(t > 0 && t < 1) 31 | %disp('t collided'); 32 | collided = 1; 33 | return; 34 | end 35 | end 36 | 37 | collided = 0; 38 | end 39 | 40 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/CheckCollisionSquare.m: -------------------------------------------------------------------------------- 1 | function [ collided ] = CheckCollisionSquare( s, f, map ) 2 | v = 0; 3 | u = 0; 4 | if(s(1) == f(1)) 5 | % Find edge going right 6 | v = 2; 7 | u = 1; 8 | else 9 | % Find edge going up 10 | v = 1; 11 | u = 2; 12 | end 13 | targetEdge = (s(v)+f(v))/2; 14 | 15 | for i=1:size(map,1) 16 | e1 = [map(i,1) map(i,3)]; 17 | e2 = [map(i,2) map(i,4)]; 18 | 19 | if(e1(v) == e2(v) && e1(v) == targetEdge) 20 | if((e1(u) > s(u) && e2(u) < f(u)) || ... 21 | (e1(u) < s(u) && e2(u) > f(u))) 22 | collided = 1; 23 | return; 24 | end 25 | end 26 | end 27 | collided = 0; 28 | end 29 | 30 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/CheckCollisionSquare2.m: -------------------------------------------------------------------------------- 1 | function [ collided ] = CheckCollisionSquare2( s, f, map ) 2 | v = 0; 3 | u = 0; 4 | if(s(1) == f(1)) 5 | % Find edge going right 6 | v = 2; 7 | u = 1; 8 | else 9 | % Find edge going up 10 | v = 1; 11 | u = 2; 12 | end 13 | targetEdge = (s(v)+f(v))/2; 14 | 15 | % Restrict edge intersection search to edges in target row/column that 16 | % are perpendicular to row/column 17 | a = (v-1)*2 + 1; % index 3 or 1 18 | edges = map((map(:,a)==targetEdge)&(map(:,a)==map(:,a+1)),:); 19 | 20 | for i=1:size(edges,1) 21 | e1 = [edges(i,1) edges(i,3)]; 22 | e2 = [edges(i,2) edges(i,4)]; 23 | 24 | if((e1(u) > s(u) && e2(u) < f(u)) || ... 25 | (e1(u) < s(u) && e2(u) > f(u))) 26 | collided = 1; 27 | return; 28 | end 29 | end 30 | collided = 0; 31 | end 32 | 33 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/distToEdge.m: -------------------------------------------------------------------------------- 1 | function [d,pt] = distToEdge(v,edge) 2 | % Computes the shortest distance to a line segment, returns distance and 3 | % closest point 4 | 5 | S = edge(3:4) - edge(1:2); 6 | S1 = v - edge(1:2); 7 | m1 = S1*S'; 8 | if (m1 <= 0 ) 9 | d = norm(v-edge(1:2)); 10 | pt = edge(1:2); 11 | else 12 | m2 = S*S'; 13 | if ( m2 <= m1 ) 14 | d = norm(v-edge(3:4)); 15 | pt = edge(3:4); 16 | else 17 | b = m1 / m2; 18 | pt = edge(1:2) + b*S; 19 | d = norm(v-pt); 20 | end 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/distanceToLineSegment.m: -------------------------------------------------------------------------------- 1 | %% distanceToLineSegment 2 | % A function to calculate the crosstrack error and tell whether we have 3 | % driven past the given line segment. 4 | % 5 | % Input: 6 | % p1 - beginning point of the line segment 7 | % p2 - end point of the line segment 8 | % x - position of the vehicle 9 | % 10 | % Output: 11 | % crosstrack_error - The distance away from the line. This value can be 12 | % either positive or negative depending on whether or 13 | % not the robot is above or below the line segment 14 | % 15 | % outside - A boolean value that tells us whether or not the robot has 16 | % travelled the length of the line segment 17 | 18 | 19 | function [crosstrack_error outside] = distanceToLineSegment(p1, p2, x) 20 | outside = 0; 21 | 22 | line_segment = p2-p1; 23 | 24 | p1_to_x = x-p1; 25 | 26 | % Compute the distance to the line as a vector, using the projection 27 | projection = p1 + (dot(line_segment,p1_to_x) / norm(line_segment)^2) * line_segment; 28 | distance = x - projection; 29 | 30 | % If the projection falls beyond the line segment then we have driven past 31 | % said line segment 32 | if (dot(line_segment,p1_to_x) / norm(line_segment)^2 > 1) 33 | outside = 1; 34 | end 35 | 36 | % Take the cross product of a vector defining the line segment with a 37 | % vector defining the robot position. Whether it is positive or negative 38 | % will tell us whether or not our robot is above or below the line segment. 39 | pos_neg = 1; 40 | cross_product = cross([line_segment 0],[p1_to_x 0]); 41 | if(cross_product(3) < 0) 42 | pos_neg = -1; 43 | end 44 | 45 | crosstrack_error = norm(distance) * pos_neg; -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/image_to_binary_map.m: -------------------------------------------------------------------------------- 1 | function [imout] = image_to_binary_map(I) 2 | imout = im2bw(I, 0.7); % Convert to 0-1 image 3 | imout = 1-(imout)'; % Convert to 0 free, 1 occupied and flip. 4 | imout = transpose(imout); 5 | end 6 | 7 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/minDistToEdges.m: -------------------------------------------------------------------------------- 1 | function [minD,minPt, d, pt, ind] = minDistToEdges(v,edges, fig) 2 | % Computes the shortest distance to a sequence of edges, which may form a 3 | % path, for example. Returns the min distance, the closest point, and the 4 | % list of distances and closest points for each edge 5 | if (nargin < 3) 6 | fig = 0; 7 | end 8 | 9 | 10 | n = length(edges(:,1)); 11 | 12 | for i=1:n 13 | [d(i), pt(i,:)] = distToEdge(v,edges(i,:)); 14 | end 15 | 16 | [minD, ind] = min(d); 17 | minPt = pt(ind,:); 18 | 19 | if (fig) 20 | figure(fig); hold on; 21 | l = [v; minPt]; 22 | plot(l(:,1),l(:,2),'g'); 23 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/rot.m: -------------------------------------------------------------------------------- 1 | function R=rot(angle, axis) 2 | % R=rot(angle (rad), axis) returns a 3x3 3 | % rotation matrix for rotating a vector about a single 4 | % axis. Setting axis = 1 rotates about the e1 axis, 5 | % axis = 2 rotates about the e2 axis, axis = 3 rotates 6 | % about the e3 axis. Uses right handed coordinates, so 7 | % rotations are counterclockwise about axis. 8 | 9 | R=eye(3); 10 | cang=cos(angle); 11 | sang=sin(angle); 12 | 13 | if (axis==1) 14 | R(2,2)=cang; 15 | R(2,3)=sang; 16 | R(3,2)=-sang; 17 | R(3,3)=cang; 18 | end; 19 | 20 | if (axis==2) 21 | R(1,1)=cang; 22 | R(1,3)=-sang; 23 | R(3,1)=sang; 24 | R(3,3)=cang; 25 | end; 26 | 27 | if (axis==3) 28 | R(1,1)=cang; 29 | R(1,2)=sang; 30 | R(2,1)=-sang; 31 | R(2,2)=cang; 32 | end; 33 | 34 | return; 35 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/geometry/rot2D.m: -------------------------------------------------------------------------------- 1 | function R = rot2D(ang) 2 | % Standard 2D rotation using 3D function 3 | R = rot(ang,3); 4 | R = R(1:2,1:2); 5 | 6 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/sampling/is_resampling.m: -------------------------------------------------------------------------------- 1 | function [xPnew, xPindnew] = is_resampling(M, xP, wx) 2 | indPnew = zeros(1, M); 3 | xPnew = zeros(1, M); 4 | 5 | WX = cumsum(wx); 6 | WX = WX / max(WX); 7 | 8 | draw = rand(1, M); 9 | for m = 1:M 10 | indPnew(m) = find(WX >= draw(m), 1); 11 | xPnew(m) = xP(indPnew(m)); 12 | end 13 | xPindnew = hist(indPnew, M); 14 | end 15 | 16 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/sampling/is_sampling.m: -------------------------------------------------------------------------------- 1 | function [indP, xP, xPind] = is_sampling(M, Gx, x) 2 | indP = zeros(1, M); 3 | xP = zeros(1, M); 4 | 5 | draw = rand(1, M); 6 | for m = 1:M 7 | indP(m) = find(Gx >= draw(m), 1); 8 | xP(m) = x(indP(m)); 9 | end 10 | xPind = hist(indP, M); 11 | end 12 | 13 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/sampling/is_weighting.m: -------------------------------------------------------------------------------- 1 | function [wx] = is_weighting(M, x, gx, fx, xP) 2 | pgx = zeros(1, M); 3 | pfx = zeros(1, M); 4 | wx = zeros(1, M); 5 | 6 | for m = 1:M 7 | pgx(m) = gx(find(x >= xP(m), 1)); 8 | pfx(m) = fx(find(x >= xP(m), 1)); 9 | wx(m) = pfx(m) / pgx(m); 10 | end 11 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/circle.m: -------------------------------------------------------------------------------- 1 | % This function draws cicles given the position of center and the radius 2 | 3 | function circle(fig,pos,r) 4 | % circle(fig, pos, r) 5 | % args: figure number, position in x,y, radius r 6 | figure(fig); 7 | rectangle('Position',[pos(1)-r,pos(2)-r,2*r,2*r], 'Curvature', [1 1], 'LineWidth',2, 'EdgeColor','k'); -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/drawbot.m: -------------------------------------------------------------------------------- 1 | function drawbot(x,y,h,scale,fig) 2 | % function drawbot(x,y,h,scale,fig) 3 | % This function plots a two wheeled robot at position x,y heading h and size scale on 4 | % figure number fig. 5 | % The default x,y = (0,0), h = 0 points to the right, and scale=1 plots a 6 | % robot with a body of radius of 1 units. 7 | 8 | % Make a circle for the body 9 | t=0:0.01:2*pi; 10 | len = length(t); 11 | bx = sin(t); 12 | by = cos(t); 13 | 14 | % Wheel locations on body 15 | wh1 = round(len/4); 16 | wh2 = round(3*len/4); 17 | 18 | % Define the wheels 19 | wwidth= 0.2; 20 | wheight = 0.4; 21 | w = [0 -wheight; wwidth -wheight; wwidth wheight; 0 wheight; 0 0]; 22 | 23 | % Body top 24 | top = round(len/2); 25 | 26 | % Top pointer 27 | pwidth = 0.1; 28 | pheight = 0.2; 29 | tp = [pwidth/2 0; 0 -pheight; -pwidth/2 0; pwidth/2 0]; 30 | 31 | % Robot outline 32 | bot = [bx(1:wh1)' by(1:wh1)'; 33 | bx(wh1)+w(:,1) by(wh1)+w(:,2); 34 | bx(wh1:wh2)' by(wh1:wh2)'; 35 | bx(wh2)-w(:,1) by(wh2)-w(:,2); 36 | bx(wh2:end)' by(wh2:end)']; 37 | 38 | point = [bx(top)+tp(:,1) by(top)+tp(:,2)]; 39 | 40 | %Size scaling 41 | bot = scale*bot; 42 | point = scale*point; 43 | 44 | % Rotation matrix 45 | R = rot(h+pi/2,3); 46 | R = R(1:2,1:2); 47 | bot = (R'*bot')'; 48 | point = (R'*point')'; 49 | 50 | % Centres 51 | bot(:,1) = bot(:,1)+x; 52 | bot(:,2) = bot(:,2)+y; 53 | point(:,1) = point(:,1)+x; 54 | point(:,2) = point(:,2)+y; 55 | 56 | % Plot 57 | figure(fig); hold on; 58 | plot(bot(:,1), bot(:,2), 'b'); 59 | plot(point(:,1), point(:,2), 'r'); 60 | axis equal 61 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/drawbox.m: -------------------------------------------------------------------------------- 1 | function drawbox(x,y,h,scale,fig) 2 | % function drawbox(x,y,h,scale,fig) 3 | % This function plots a box at position x,y heading h and size scale on 4 | % figure number fig. 5 | 6 | 7 | % Car outline 8 | box = [-1 -0.5; 1 -0.5; 1 0.5; -1 0.5; -1 -0.5]; 9 | 10 | %Size scaling 11 | box = scale*box; 12 | 13 | % Rotation matrix 14 | R = rot2D(h); 15 | box = (R*box')'; 16 | 17 | % Centre 18 | box(:,1) = box(:,1)+x; 19 | box(:,2) = box(:,2)+y; 20 | 21 | % Plot 22 | figure(fig); 23 | plot(box(:,1), box(:,2), 'b','LineWidth', 2); 24 | axis equal 25 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/drawcar.m: -------------------------------------------------------------------------------- 1 | function drawcar(x,y,h,delta,scale,fig) 2 | % function drawcar(x,y,h,delta,scale,fig) 3 | % This function plots a car at position x,y heading h with steering angle delta 4 | % and size scale on figure number fig. 5 | % The default x,y = (0,0), h = 0, delta = 0 drives to the right, and scale=1 plots a 6 | % car with a rectangular body of length 1 units and width 0.5 units. 7 | 8 | % Define the body shape 9 | bwidth= 0.5; 10 | blength = 1; 11 | body = [-blength/2 -bwidth/2; blength/2 -bwidth/2; blength/2 bwidth/2; -blength/2 bwidth/2; -blength/2 -bwidth/2]; 12 | 13 | % Define the wheel shape 14 | wwidth= 0.1; 15 | wlength = 0.2; 16 | wheel = [-wlength/2 -wwidth/2; wlength/2 -wwidth/2; wlength/2 wwidth/2; -wlength/2 wwidth/2; -wlength/2 -wwidth/2]; 17 | R = rot2D(delta); 18 | R = R(1:2,1:2) 19 | 20 | % rotate the wheel coordinates into the inertial frame using R' 21 | fwheel = (R'*wheel')'; 22 | 23 | % Define car 24 | n = length(wheel(:,1)); 25 | wheel1 = wheel + [body(1,1)*ones(n,1),body(1,2)*ones(n,1)]; 26 | wheel2 = fwheel + [body(2,1)*ones(n,1),body(2,2)*ones(n,1)]; 27 | wheel3 = fwheel + [body(3,1)*ones(n,1),body(3,2)*ones(n,1)]; 28 | wheel4 = wheel + [body(4,1)*ones(n,1),body(4,2)*ones(n,1)]; 29 | 30 | car = [body;NaN NaN; 31 | wheel1;NaN NaN; 32 | wheel2;NaN NaN; 33 | wheel3;NaN NaN; 34 | wheel4]; 35 | 36 | %Size scaling 37 | car = scale*car; 38 | 39 | % Rotation matrix 40 | R = rot2D(h); 41 | car = (R'*car')'; 42 | 43 | % Centre 44 | car(:,1) = car(:,1)+x; 45 | car(:,2) = car(:,2)+y; 46 | 47 | % Plot 48 | figure(fig); 49 | plot(car(:,1), car(:,2), 'b'); 50 | axis equal 51 | end 52 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/flyover/plot_ellipse.m: -------------------------------------------------------------------------------- 1 | function plot_ellipse(mu, S) 2 | mu_pos = [mu(1) mu(3)]; 3 | S_pos = [S(1,1) S(1,3); S(3,1) S(3,3)]; 4 | error_ellipse(S_pos, mu_pos, 0.75); 5 | error_ellipse(S_pos, mu_pos, 0.95); 6 | end 7 | 8 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/flyover/plot_errors.m: -------------------------------------------------------------------------------- 1 | function plot_errors(figure_index, T, x, mu_S, mu_Su, muP_S) 2 | figure(figure_index); 3 | clf; 4 | hold on; 5 | 6 | % plot ekf 7 | e = sqrt((x(1, 2:end) - mu_S(1, 2:end)).^2+(x(3, 2:end)-mu_S(3, 2:end)).^2); 8 | plot(T(2:end), e, 'b', 'LineWidth', 1.5); 9 | 10 | % plot ukf 11 | eu = sqrt((x(1, 2:end) - mu_Su(1, 2:end)).^2 + (x(3, 2:end) - mu_Su(3, 2:end)).^2); 12 | plot(T(2:end), eu, 'g', 'LineWidth', 1.5); 13 | 14 | % plot pf 15 | ep = sqrt((x(1, 2:end) - muP_S(1, 2:end)).^2 +(x(3, 2:end) - muP_S(3, 2:end)).^2); 16 | plot(T(2:end), ep, 'm', 'LineWidth', 1.5); 17 | 18 | % plot details 19 | title('Position Estimation Errors for EKF, UKF and PF') 20 | xlabel('Time (s)'); 21 | ylabel('X-Z Position Error (m)'); 22 | legend('EKF', 'UKF', 'Particle'); 23 | end 24 | 25 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/flyover/plot_flyover.m: -------------------------------------------------------------------------------- 1 | function plot_flyover(figure_index, t, x, mu_S, mu_Su, muP_S, X) 2 | figure(figure_index); 3 | clf; 4 | hold on; 5 | 6 | % true state 7 | plot(x(1,2:t), x(3,2:t), 'ro--') 8 | 9 | % EKF 10 | plot(mu_S(1,2:t), mu_S(3,2:t), 'bx--') 11 | %plot_ellipse(mu, S); 12 | 13 | % UKF 14 | plot(mu_Su(1,2:t), mu_Su(3,2:t), 'gx--') 15 | %plot_ellipse(mu_Su, S_u); 16 | 17 | % PF 18 | plot(muP_S(1,2:t), muP_S(3,2:t), 'mx--') 19 | plot(X(1,1:10:end), X(3,1:10:end), 'm.') 20 | 21 | % plot ground 22 | plot(0, 0, 'bx', 'MarkerSize', 6, 'LineWidth', 2) 23 | plot([20 -1], [0 0],'b--') 24 | 25 | % plot details 26 | title('Flyover Example') 27 | axis equal 28 | axis([-5 20 -1 10]) 29 | legend('True state', 'EKF', 'UKF', 'PF') 30 | end 31 | 32 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/importance_sampling/plot_distribution_importance.m: -------------------------------------------------------------------------------- 1 | function plot_distribution_importance(figure_index, M, x, fx, gx, xPnew) 2 | figure(figure_index); 3 | clf; 4 | 5 | subplot(2,1,1); 6 | hold on; 7 | 8 | plot(x, fx/0.01,'b'); 9 | plot(x, gx/0.01,'r'); 10 | 11 | title('Distributions, and importance sampled f(x)') 12 | legend('f(x)', 'g(x)') 13 | axis([-3 2 0 0.7]) 14 | 15 | subplot(2,1,2); 16 | hold on; 17 | [xPnewH, xnewH] = hist(xPnew, 200); 18 | plot(xnewH, xPnewH ./ M, 'b'); 19 | 20 | xlabel('x'); 21 | ylabel('# samples'); 22 | axis([-3, 2, 0, 1.1 * max(xPnewH ./ M)]) 23 | end 24 | 25 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/importance_sampling/plot_initial_sample.m: -------------------------------------------------------------------------------- 1 | function plot_initial_sample(figure_index, M, x, fx, gx, xP) 2 | figure(figure_index); 3 | clf; 4 | 5 | % plot of both distributions, and initial sample of g(x) 6 | % f(x) and g(x) 7 | subplot(3,1,1); 8 | hold on; 9 | plot(x, fx / 0.01, 'b'); 10 | plot(x, gx / 0.01, 'r'); 11 | title('Distributions, and Samples from g(x)') 12 | legend('f(x)', 'g(x)') 13 | axis([-3 2 0 0.7]) 14 | 15 | % actual samples plotted as bars, solidity represents density 16 | subplot(3, 1, 2); 17 | hold on; 18 | for m=1:25:M 19 | plot([xP(m);xP(m)],[0 1],'r') 20 | end 21 | ylabel('Samples'); 22 | axis([-3 2 0 1]) 23 | 24 | % histogram of samples into 200 bins 25 | subplot(3,1,3); 26 | hold on; 27 | [xPH, xH] = hist(xP, 200); 28 | plot(xH, xPH ./ M, 'r'); 29 | xlabel('x'); 30 | ylabel('# samples'); 31 | axis([-3, 2, 0, 1.1 * max(xPH ./ M)]) 32 | end 33 | 34 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/importance_sampling/plot_sample_weights.m: -------------------------------------------------------------------------------- 1 | function plot_sample_weights(figure_index, M, x, fx, gx, xP, wx) 2 | figure(figure_index); 3 | clf; 4 | subplot(2,1,1); 5 | hold on; 6 | 7 | plot(x, fx / 0.001, 'b'); 8 | plot(x, gx / 0.001, 'r'); 9 | plot(x, fx ./ gx, 'g'); 10 | 11 | title('Weights for each sample of g(x)') 12 | legend('f(x)', 'g(x)', 'f(x)/g(x)') 13 | axis([-3 2 0 7]) 14 | subplot(2,1,2); 15 | hold on; 16 | 17 | for m = 1:25:M 18 | plot([xP(m); xP(m)], [0 wx(m)], 'g') 19 | end 20 | xlabel('x') 21 | ylabel('Weights f(x)/g(x)') 22 | axis([-3 2 0 max(wx)]) 23 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/ekf_approximation.m: -------------------------------------------------------------------------------- 1 | function [muLN, SLN, yLN, pyLN] = ekf_approximation(L, S, mu) 2 | G = 1 / ((mu + 1 / 2)^2 + 1); 3 | muLN = atan(mu + 0.5); 4 | SLN = G * S * G'; 5 | yLN = [muLN-L*sqrt(SLN):0.01:muLN+L*sqrt(SLN)]; 6 | pyLN = normpdf(yLN, muLN, sqrt(SLN)); 7 | end 8 | 9 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/linear_transform.m: -------------------------------------------------------------------------------- 1 | function [yL, muL, SL, pyL] = linear_transform(x, mu, S) 2 | % Linear transformation 3 | A = 2; 4 | b = -2; 5 | yL = A * x + b; 6 | 7 | % Resulting Gaussian distribution 8 | muL = A * mu + b; 9 | SL = A * S * A'; 10 | pyL = normpdf(yL, muL, SL); 11 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/nonlinear_transform.m: -------------------------------------------------------------------------------- 1 | function [yN, YN, pyN, muN, yNG, pyNG] = nonlinear_transform(x, xS, n, L) 2 | yN = atan(x + 0.5); % Nonlinear transformation of pdf 3 | yNS = atan(xS + 0.5); % Nonlinear transformation of samples 4 | 5 | % Resulting distribution 6 | m = 100; % Number of bins to use 7 | [d, YN] = hist(yNS, m); % Histogram of results 8 | binw = (max(YN) - min(YN)) / (m - 1); % Bin width 9 | pyN = d / n / binw; % Resulting distribution 10 | 11 | % Gaussian approximation to resulting distribution 12 | muN = mean(yNS); % Mean of samples of transformed distribution 13 | SN = var(yNS); % Covariance of samples of transformed distribution 14 | yNG = [muN-L*sqrt(SN):0.01:muN+L*sqrt(SN)]; 15 | pyNG = normpdf(yNG, muN, sqrt(SN)); 16 | end 17 | 18 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/plot_linear_transform.m: -------------------------------------------------------------------------------- 1 | function plot_linear_transform(figure_index, x, px, yL, pyL) 2 | % Linear plot 3 | figure(figure_index); 4 | clf; 5 | 6 | % Original distribution 7 | subplot(2,2,4) 8 | plot(x, px, 'b'); 9 | title('Original') 10 | xlabel('x') 11 | ylabel('p(x)') 12 | axis(1.2*[min(x) max(x) min(px) max(px)]) 13 | 14 | % Transformation function 15 | subplot(2,2,2) 16 | plot(x, yL, 'g') 17 | title('Linear Transformation y=Ax+b') 18 | xlabel('x') 19 | ylabel('y') 20 | axis(1.2*[min(x) max(x) min(yL) max(yL)]) 21 | axis equal 22 | 23 | % Resulting distribution 24 | subplot(2,2,1) 25 | plot(pyL,yL,'r'); 26 | title('Final') 27 | xlabel('p(y)') 28 | ylabel('y') 29 | axis(1.2*[min(pyL) max(pyL) min(yL) max(yL)]) 30 | end 31 | 32 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/plot_nonlinear_transform.m: -------------------------------------------------------------------------------- 1 | function plot_nonlinear_transform(figure_index, x, px, yN, pyN, YN, pyNG, yNG, muN) 2 | figure(figure_index); 3 | clf; 4 | 5 | subplot(2,2,4) 6 | plot(x, px, 'b'); 7 | title('Original') 8 | xlabel('x') 9 | ylabel('p(x)') 10 | axis(1.2*[min(x) max(x) min(px) max(px)]) 11 | 12 | subplot(2,2,2) 13 | plot(x, yN, 'g') 14 | title('Nonlinear Transformation y=tan^{-1}(x+1/2)') 15 | xlabel('x') 16 | ylabel('y') 17 | axis(1.2*[min(x) max(x) min(yN) max(yN)]) 18 | 19 | subplot(2, 2, 1); 20 | hold on; 21 | plot(pyN, YN, 'r'); 22 | plot(pyNG, yNG, 'm--'); 23 | plot([0 max(pyNG)], [muN muN], 'm--') 24 | axis(1.2*[min(pyN) max(pyN) min(yN) max(yN)]) 25 | 26 | title('Resulting Distribution') 27 | xlabel('p(y)') 28 | ylabel('y') 29 | end 30 | 31 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/plot_resulting_distributions.m: -------------------------------------------------------------------------------- 1 | function plot_resulting_distributions(figure_index, X, Y, yN, YN, pyN, yNG, pyNG, yLN, pyLN, yNU, pyNU, muN, muU, muLN) 2 | figure(figure_index); 3 | clf; 4 | hold on; 5 | 6 | plot(YN,pyN,'r', 'LineWidth', 1.5); 7 | plot(yNG,pyNG,'m--', 'LineWidth', 1.5); 8 | plot(yLN,pyLN,'g--','LineWidth', 1.5); 9 | plot(yNU,pyNU,'c--','LineWidth', 1.5); 10 | plot([muN muN],[0 max(pyNG)],'m--','LineWidth', 1.5) 11 | plot([muLN muLN], [0 max(pyLN)],'g--','LineWidth', 1.5) 12 | plot([muU muU], [0 max(pyNU)],'c--','LineWidth', 1.5) 13 | plot(X,0.001*ones(1,length(X)), 'cx', 'MarkerSize', 8, 'LineWidth', 2); 14 | plot(Y,0.001*ones(1,length(Y)), 'co', 'MarkerSize', 8, 'LineWidth', 2); 15 | 16 | title('Resulting Distribution') 17 | xlabel('y') 18 | ylabel('p(y)') 19 | legend('NL distribution', 'Best Gaussian Fit', 'EKF approx', 'UKF approx'); 20 | axis(1.2*[2*min(yN) 2*max(yN) min(pyN) max(pyN)]) 21 | end 22 | 23 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/nonlinear/unscented_approximation.m: -------------------------------------------------------------------------------- 1 | function [X, Y, muU, yNU, pyNU] = unscented_approximation(L, S, mu) 2 | n = 1; 3 | alpha = 2; 4 | kappa = 1; 5 | beta = 2; 6 | lambda = alpha^2 * (n + kappa) - n; 7 | 8 | X(:,1) = mu; 9 | Y(:,1) = atan(X(:,1) + 0.5); 10 | nlS = sqrt((n + lambda) * S); 11 | for i = 1:n 12 | X(:, i + 1) = mu + nlS(:, i); 13 | Y(:, i + 1) = atan(X(:, i + 1) + 0.5); 14 | X(:, n + i + 1) = mu - nlS(:, i); 15 | Y(:, n + i + 1) = atan(X(:, n + i + 1) + 0.5); 16 | end 17 | wm(1) = lambda / (n + lambda); 18 | wc(1) = wm(1) + (1 - alpha^2 + beta) + (1 - alpha^2 + beta); 19 | wm(2:2 * n + 1) = 1 / (2 * (n + lambda)); 20 | wc(2:2 * n + 1) = 1 / (2 * (n + lambda)); 21 | 22 | muU = zeros(n,1); 23 | SU = zeros(n,n); 24 | for i = 1:2 * n + 1 25 | muU = muU + wm(i) * Y(:, i); 26 | end 27 | for i = 1:2 * n + 1 28 | SU = SU + wc(i) * (Y(:, i) - muU) * (Y(:, i) - muU)'; 29 | end 30 | yNU = [muU-L*sqrt(SU):0.01:muU+L*sqrt(SU)]; 31 | pyNU = normpdf(yNU, muU, sqrt(SU)); 32 | end 33 | 34 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/particle_filter/plot_first_time_step.m: -------------------------------------------------------------------------------- 1 | function plot_first_time_step(t, Q, mup, mu, Sp, S, X, X0, Xp, y) 2 | L = 5; 3 | 4 | plot_prior_belief(1, L, mu, S, X0); 5 | plot_prediction(2, L, mu, mup, S, Sp, Xp); 6 | plot_measurement(3, t, L, mu, mup, S, Sp, Q, X, y); 7 | end 8 | 9 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/particle_filter/plot_measurement.m: -------------------------------------------------------------------------------- 1 | function plot_measurement(figure_index, t, L, mu, mup, S, Sp, Q, X, y) 2 | figure(figure_index); 3 | clf; 4 | subplot(2,1,1); 5 | hold on; 6 | 7 | z = [(mup - L * sqrt(Sp)):(0.01):(mup + L * sqrt(Sp))]; 8 | plot(z, normpdf(z, mup, Sp), 'r'); 9 | 10 | z = [(y(t) - L * sqrt(Q)):(0.01):(y(t) + L * sqrt(Q))]; 11 | plot(z, normpdf(z, y(t), Q), 'g'); 12 | 13 | z = [(mu - L * sqrt(S)):(0.01):(mu + L * sqrt(S))]; 14 | plot(z,normpdf(z, mu, S), 'm'); 15 | 16 | axis([0 15 0 0.6]); 17 | title('Prediction, Measurement & Belief') 18 | legend('Prediction','Measurement', 'Belief') 19 | subplot(2,1,2); hold on; 20 | for m = 1:length(X) 21 | plot([X(m); X(m)], [0 1], 'm') 22 | end 23 | axis([0 15 0 1]); 24 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/particle_filter/plot_prediction.m: -------------------------------------------------------------------------------- 1 | function plot_prediction(figure_index, L, mu, mup, S, Sp, Xp) 2 | figure(figure_index); 3 | clf; 4 | subplot(2,1,1); 5 | hold on; 6 | 7 | z = [(mu - L * sqrt(S)):(0.01):(mu + L * sqrt(S))]; 8 | plot(z, normpdf(z, mu, S), 'b'); 9 | 10 | z = [(mup - L * sqrt(Sp)):(0.01):(mup + L * sqrt(Sp))]; 11 | plot(z, normpdf(z, mup, Sp),'r'); 12 | 13 | title('Prior & Prediction') 14 | legend('Prior', 'Prediction') 15 | axis([0 15 0 0.4]) 16 | subplot(2, 1, 2); 17 | hold on; 18 | for m = 1:length(Xp) 19 | plot([Xp(m); Xp(m)], [0 1], 'r') 20 | end 21 | axis([0 15 0 1]) 22 | end -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/particle_filter/plot_prior_belief.m: -------------------------------------------------------------------------------- 1 | function plot_prior_belief(figure_index, L, mu, S, X0) 2 | figure(figure_index); 3 | clf; 4 | subplot(2,1,1); 5 | hold on; 6 | 7 | z = [(mu - L * sqrt(S)):(0.01):(mu + L * sqrt(S))]; 8 | plot(z, normpdf(z, mu, S), 'b'); 9 | plot([0 15], [1/15 1/15]) 10 | 11 | title('Priors') 12 | axis([5 15 0 0.4]) 13 | subplot(2, 1, 2); 14 | hold on; 15 | for m = 1:length(X0) 16 | plot([X0(m); X0(m)], [0 1], 'b') 17 | end 18 | axis([5 15 0 1]) 19 | end 20 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/particle_filter/plot_trajectory.m: -------------------------------------------------------------------------------- 1 | function plot_trajectory(figure_index, T, x, y, mu_S, muP_S, M) 2 | figure(figure_index); 3 | clf; 4 | hold on; 5 | 6 | plot(T, x(2:end), 'b') 7 | %plot(T,y,'rx') 8 | %plot(T,u,'g'); 9 | plot(T, mu_S, 'r--') 10 | plot(T, muP_S, 'co--') 11 | plot(T, 2 * ones(size(T)), 'm--'); 12 | plot(T, 10 * ones(size(T)), 'm--'); 13 | 14 | title(sprintf('State and estimates, M=%d', M)); 15 | legend('State', 'Kalman Estimate', 'Particle Estimate') 16 | end 17 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_cell_map.m: -------------------------------------------------------------------------------- 1 | function plot_cell_map(map, M, N, fig) 2 | % Plots a cell map in black and white 3 | % Input: 4 | % [map] = Cell map in matrix form 5 | % M = Height of map 6 | % N = Width of map 7 | % fig = handle of figure to generate 8 | % Output: 9 | % Plot window showing cell map in black and white 10 | 11 | figure(fig); 12 | clf; 13 | hold on; 14 | image(100 * (1 - map)); 15 | colormap(gray); 16 | axis([0 N 0 M]) 17 | title('Cell Map'); -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_inverse_mm.m: -------------------------------------------------------------------------------- 1 | function plot_inverse_mm(og_mm, M, N, x, phi_m, r_m, fig) 2 | % Plots the occupancy probabilities of a single measurement model timestep 3 | % Input: 4 | % [og_mm] = (O)ccupancy (g)rid of the (m)easurement (m)odel 5 | % M = Height of map 6 | % N = Width of map 7 | % [x] = Robot state vector [x position; y position; heading] 8 | % [phi_m] = Array of measurement angles relative to robot 9 | % [r_m] = Array of (r)ange (m)easurements 10 | % fig = handle of figure to generate 11 | % Output: 12 | % Plot window showing occupancy window of inverse measurement model 13 | 14 | figure(fig); 15 | clf; 16 | hold on; 17 | image(100 * (og_mm)); 18 | colormap(gray); 19 | for i = 1:length(r_m) 20 | plot(x(2) + r_m(i)*sin(phi_m(i) + x(4)), x(1) + r_m(i)*cos(phi_m(i) + x(4)), 'r.') 21 | end 22 | axis([0 N 0 M]) 23 | title('Measurements and inverse measurement model'); -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_localization_results.m: -------------------------------------------------------------------------------- 1 | function plot_localization_results(figure_index, meas, map, t, x, X, y, mf, D) 2 | % plot setup 3 | figure(figure_index); 4 | clf; 5 | hold on; 6 | 7 | % plot 8 | plot(map(:, 1), map(:,2), 'go', 'MarkerSize', 10, 'LineWidth', 2); 9 | plot(mf(1, t), mf(2, t),'mx', 'MarkerSize', 10, 'LineWidth', 2); 10 | plot(x(1, 1:t), x(2, 1:t), 'ro--'); 11 | 12 | switch(meas) 13 | case 1 % 1 - range 14 | circle(1,x(1:2, t), y(1, t)); 15 | case 2 % 2 - bearing 16 | plot([x(1, t) x(1, t) + 10 * cos(y(1, t) + x(3, t))], [x(2, t) x(2, t) + 10 * sin(y(1, t) + x(3, t))], 'c'); 17 | case 3 % 3 - both 18 | plot([x(1, t) x(1, t) + y(1, t) * cos(y(2, t) + x(3, t))], [x(2, t) x(2, t) + y(1, t) * sin(y(2, t) + x(3, t))], 'c'); 19 | end 20 | 21 | for dd = 1:D 22 | plot(X(1, dd), X(2, dd), 'b.') 23 | end 24 | end 25 | 26 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_localization_state.m: -------------------------------------------------------------------------------- 1 | function plot_localization_state(figure_index, map, x, X, D) 2 | figure(figure_index); 3 | clf; 4 | hold on; 5 | 6 | plot(map(:, 1), map(:, 2), 'go', 'MarkerSize', 10, 'LineWidth', 2); 7 | plot(x(1, 1), x(2, 1), 'ro--') 8 | for dd = 1:D 9 | plot(X(1, dd), X(2, dd), 'b.') 10 | end 11 | 12 | axis equal 13 | end 14 | -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_occupancy_grid.m: -------------------------------------------------------------------------------- 1 | function plot_occupancy_grid(og, M, N, fig) 2 | % Plots an occupancy grid with belief probabilities shown in grayscale 3 | % Input: 4 | % [og] = Existing occupancy grid in log odds form 5 | % M = Height of map 6 | % N = Width of map 7 | % fig = handle of figure to generate 8 | % Output: 9 | % Plot window showing occupancy grid 10 | 11 | figure(fig); 12 | clf; 13 | hold on; 14 | image(100 * (og)); 15 | colormap(gray); 16 | axis([0 N 0 M]) 17 | title('Current occupancy grid map') -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/plot_robot_path.m: -------------------------------------------------------------------------------- 1 | function plot_robot_path(x, t, fig) 2 | % Plots a robot path on a given figure handle 3 | % Input: 4 | % [x] = Matrix of robot state vectors, time is the other dimension 5 | % [t] = Current simulation time index 6 | % fig = handle of figure to generate 7 | % Output: 8 | % Plot window showing robot path 9 | 10 | figure(fig); 11 | plot(x(2, 1:t), x(1, 1:t), 'bx-') -------------------------------------------------------------------------------- /matlab_simulation/09-utilities/visualization/quad_starmac.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/09-utilities/visualization/quad_starmac.mat -------------------------------------------------------------------------------- /matlab_simulation/10-environments/create_a_map.m: -------------------------------------------------------------------------------- 1 | function [ map ] = create_a_map( M, N ) 2 | % This function creates a random map with obstacles 3 | % M -- Height of the map 4 | % N -- Width of the map 5 | 6 | % Map initialization 7 | map = zeros(M,N); 8 | 9 | % Boundings of map 10 | map(1,:) = 1; 11 | map(M,:) = 1; 12 | map(:,1) = 1; 13 | map(:,N) = 1; 14 | 15 | % Obstacles 16 | map(13:round(17+2*randn(1)),22:26) = 1; 17 | map(15:round(20+2*randn(1)),40:43) = 1; 18 | map(30:34,5:7) = 1; 19 | map(40:45,round(26+1*randn(1)):29) = 1; 20 | map(29:30,15:32) = 1; 21 | map(23:35,23:24) = 1; 22 | 23 | end 24 | -------------------------------------------------------------------------------- /matlab_simulation/10-environments/makegallerymap.m: -------------------------------------------------------------------------------- 1 | function [boundary, obs, tour] = makegallerymap() 2 | %% Gallery map 3 | % Constructs a simple gallery with obstacles for motion planning 4 | 5 | boundary = [0 0; 30 0; 30 30; 0 30]; 6 | obs{1} = [3 14; 3 16; 8 16; 8 14]; 7 | obs{2} = [12 14; 12 16; 18 16; 18 14]; 8 | obs{3} = [21 14; 21 16; 27 16; 27 14]; 9 | obs{4} = [20 24; 22 24; 24 26; 26 24; 24 28]; 10 | obs{5} = [10 0; 10 10; 20 10; 20 0]; 11 | obs{6} = [30 20; 25 20; 25 22; 30 22]; 12 | obs{7} = [15 30; 15 22; 13 22; 13 30]; 13 | obs{8} = [5 20; 5 22; 20 22; 20 20]; 14 | 15 | 16 | galleryMap = [boundary; boundary(1,:); NaN NaN;]; 17 | for i=1:length(obs) 18 | galleryMap = [galleryMap; obs{i}; obs{i}(1,:); NaN NaN;]; 19 | end 20 | tour = [4 28; 5 3; 27 27; 26 5]; 21 | 22 | figure(1); clf; hold on; 23 | plot(galleryMap(:,1),galleryMap(:,2)); 24 | plot(tour(:,1), tour(:,2), 'rx'); 25 | title('Gallery Tour'); 26 | axis([-1 31 -1 31]) 27 | 28 | return; 29 | -------------------------------------------------------------------------------- /matlab_simulation/10-environments/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'facecolor','none','linestyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/11-datasets/E5_Third_Floor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/11-datasets/E5_Third_Floor.png -------------------------------------------------------------------------------- /matlab_simulation/11-datasets/funnymap.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/11-datasets/funnymap.jpg -------------------------------------------------------------------------------- /matlab_simulation/11-datasets/gyroData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/11-datasets/gyroData.mat -------------------------------------------------------------------------------- /matlab_simulation/12-test/runAllLectureExamples.m: -------------------------------------------------------------------------------- 1 | % Run all lecture examples 2 | 3 | filelist = dir('../01-examples_lecture/**/*.m') 4 | 5 | for i=1:length(filelist) 6 | filelist = dir('../01-examples_lecture/**/*.m'); 7 | str = sprintf('Running %s',filelist(i).name); 8 | disp(str) 9 | run(filelist(i).name) 10 | end 11 | -------------------------------------------------------------------------------- /matlab_simulation/12-test/test_drawquadrotor.m: -------------------------------------------------------------------------------- 1 | %% Test drawquadrotor.m 2 | 3 | load('quad_starmac.mat') 4 | figure(1); clf; hold on; 5 | drawquadrotor(qm_fnum, qm_xyz, 1, 0, 0, 0, 0, 0,[0.6 0.1 0.1]); 6 | plot3(0,0,0, 'go', 'MarkerSize', 6, 'LineWidth', 2); % Draw object 7 | xlabel('X') 8 | ylabel('Y') 9 | zlabel('Z') 10 | axis([-5 5 -5 5 -5 5]) 11 | view(0, 90); 12 | grid on -------------------------------------------------------------------------------- /matlab_simulation/MobileRoboticsSetup.m: -------------------------------------------------------------------------------- 1 | function MobileRoboticsSetup(varargin); 2 | %% Path setup for ME 597 Code library 3 | % Sets up the path for all code in the ME 597 library, and saves the new 4 | % path to automatically reload on Matlab restart. Run once. 5 | % If you add directories to the library, you can also add them to the 6 | % dirlist if you like. 7 | % 8 | % To remove installed directories from path, use 9 | % MobileRoboticsSetup('remove'); 10 | % 11 | % Tested in Windows, should work in Linux/Mac 12 | if (nargin == 1) 13 | mode = varargin{1}; 14 | else 15 | mode = 'setup'; 16 | end 17 | 18 | % Find base directory 19 | base = pwd; 20 | cd(base); 21 | 22 | % Set slash convention 23 | os = getenv('OS'); 24 | sep = '/'; 25 | if (strcmp(os, 'Windows_NT')) 26 | sep = '\'; 27 | end 28 | 29 | % Add base and all subdirectories to path 30 | if (strcmp(mode, 'remove')) 31 | rmpath(genpath(base)); 32 | !rm pathdef.m 33 | disp('Removed all directories from path'); 34 | else 35 | addpath(genpath(base)); 36 | % remove code v1.0 to avoid duplication, can be eliminated when v2.0 37 | % complete 38 | oldbase = strcat(base,sep,'code v1.0'); 39 | rmpath(genpath(oldbase)); 40 | savepath pathdef.m 41 | disp('Added all directories to path'); 42 | end 43 | 44 | disp('Setup complete') 45 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-CollisionAvoidance/constraints.m: -------------------------------------------------------------------------------- 1 | function [C, Ceq] = constraints(x) 2 | % Returns vectors C, Ceq which define the value of the constraints. fmincon 3 | % uses these to determine whether a solution x is feasible and which 4 | % constraints it is currently violating 5 | % C(x) <= 0 and Ceq(x) = 0 6 | 7 | global nv nx n N Tr ds dt 8 | 9 | % Collision Avoidance inequality constraints 10 | % There are nv(nv-1)/2 pairs of vehicles and Tr time steps to evaluate (do 11 | % not include the initial positions, as they are already fixed). 12 | C = zeros(nv*(nv-1)/2*Tr,1); 13 | k = 1; % Increment constraint evaluations (short cut to computing proper index) 14 | for t = 2:Tr+1 % For each time step after initial 15 | for i = 1:nv % For each vehicle i 16 | for j = i+1:nv % For each other vehicle j greater than i 17 | xcuri = x(N*(t-1)+n*(i-1)+1:N*(t-1)+n*(i-1)+2); % Position i 18 | xcurj = x(N*(t-1)+n*(j-1)+1:N*(t-1)+n*(j-1)+2); % Position j 19 | C(k) = ds - norm(xcuri-xcurj); % min distance constraint 20 | k = k +1; 21 | end 22 | end 23 | end 24 | 25 | 26 | 27 | % Dynamics, nonlinear equality constraints, nv vehicles times nx states 28 | % times Tr periods (from 0 to 1, 1 to 2,... Tr to Tr+1) 29 | Ceq = zeros(nv*nx*Tr,1); 30 | k = 1; % Increment constraint evaluations (short cut) 31 | for t = 1:Tr 32 | for i = 1:nv 33 | xprev = x(N*(t-1)+n*(i-1)+1:N*(t-1)+n*i); % Previous state and inputs 34 | xcur = x(N*(t)+n*(i-1)+1:N*(t)+n*i); % Current state and inputs 35 | % Dynamic constraints on the three states 36 | Ceq(k:k+2) = xcur(1:3)-xprev(1:3)-dt*[cos(xprev(3))*xprev(4); sin(xprev(3))*xprev(4); xprev(5)]; 37 | k = k+3; 38 | end 39 | end 40 | 41 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-CollisionAvoidance/cost.m: -------------------------------------------------------------------------------- 1 | function f = cost(x) 2 | % Returns the total cost for a given x, used by fmincon to evaluate 3 | % possible solutions 4 | 5 | global Tr nv N n xd beta % Requires these global variables (does not change them) 6 | 7 | % Initialize cost to zero, and add all the contributions together with beta 8 | % and 1-beta weighting on the two cost elements 9 | f = 0; 10 | for t = 2:Tr+1 % For each timestep after the initial 11 | for i = 1:nv % For each vehicle 12 | 13 | % current vehicle state and inputs 14 | xcur = x(N*(t-1)+n*(i-1)+1:N*(t-1)+n*(i-1)+n); 15 | 16 | % Position error cost, quadratic distance from desired position 17 | f = f + beta*((xd(1,t-1,i) - xcur(1))^2+ (xd(2,t-1,i) - xcur(2))^2); 18 | 19 | % Turn rate control input cost, quadratic sum of turn rate inputs 20 | f = f + (1-beta)*xcur(5)^2; 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-GalleryPField/gallerypfield/distToEdge.m: -------------------------------------------------------------------------------- 1 | function [d,pt] = distToEdge(v,edge) 2 | % Computes the shortest distance to a line segment, returns distance and 3 | % closest point 4 | 5 | S = edge(3:4) - edge(1:2); 6 | S1 = v - edge(1:2); 7 | m1 = S1*S'; 8 | if (m1 <= 0 ) 9 | d = norm(v-edge(1:2)); 10 | pt = edge(1:2); 11 | else 12 | m2 = S*S'; 13 | if ( m2 <= m1 ) 14 | d = norm(v-edge(3:4)); 15 | pt = edge(3:4); 16 | else 17 | b = m1 / m2; 18 | pt = edge(1:2) + b*S; 19 | d = norm(v-pt); 20 | end 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-GalleryPField/gallerypfield/gallery.m: -------------------------------------------------------------------------------- 1 | % Gallery map 2 | 3 | boundary = [0 0; 10 0; 10 10; 20 10; 20 0; 30 0; 30 20; 25 20; 25 22; 30 22; 30 30; 15 30; 15 22; 20 22; 20 20; 5 20; 5 22; 13 22;13 30; 0 30]; 4 | obstacle1 = [3 14; 3 16; 8 16; 8 14]; 5 | obstacle2 = [12 14; 12 16; 18 16; 18 14]; 6 | obstacle3 = [21 14; 21 16; 27 16; 27 14]; 7 | obstacle4 = [20 24; 22 24; 24 26; 26 24; 24 28]; 8 | 9 | galleryMap = [boundary; boundary(1,:); NaN NaN; 10 | obstacle1; obstacle1(1,:); NaN NaN; 11 | obstacle2; obstacle2(1,:); NaN NaN; 12 | obstacle3; obstacle3(1,:); NaN NaN; 13 | obstacle4; obstacle4(1,:); NaN NaN]; 14 | 15 | tour = [4 28; 5 3; 27 27; 26 5]; 16 | 17 | figure(1); clf; hold on; 18 | plot(galleryMap(:,1),galleryMap(:,2)); 19 | plot(tour(:,1), tour(:,2), 'rx'); 20 | title('Gallery Tour'); 21 | axis([-1 31 -1 31]) 22 | 23 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-GalleryPField/gallerypfield/minDistToEdges.m: -------------------------------------------------------------------------------- 1 | function [minD,minPt, d, pt, ind] = minDistToEdges(v,edges, fig) 2 | % Computes the shortest distance to a sequence of edges, which may form a 3 | % path, for example. Returns the min distance, the closest point, and the 4 | % list of distances and closest points for each edge 5 | if (nargin < 3) 6 | fig = 0; 7 | end 8 | 9 | 10 | n = length(edges(:,1)); 11 | 12 | for i=1:n 13 | [d(i), pt(i,:)] = distToEdge(v,edges(i,:)); 14 | end 15 | 16 | [minD, ind] = min(d); 17 | minPt = pt(ind,:); 18 | 19 | if (fig) 20 | figure(fig); hold on; 21 | l = [v; minPt]; 22 | plot(l(:,1),l(:,2),'g'); 23 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-GalleryPField/gallerywavefront/gallery.m: -------------------------------------------------------------------------------- 1 | % Gallery map Wavefront planning 2 | 3 | boundary = [0 0; 10 0; 10 10; 20 10; 20 0; 30 0; 30 20; 25 20; 25 22; 30 22; 30 30; 15 30; 15 22; 20 22; 20 20; 5 20; 5 22; 13 22;13 30; 0 30]; 4 | obstacle1 = [3 14; 3 16; 8 16; 8 14]; 5 | obstacle2 = [12 14; 12 16; 18 16; 18 14]; 6 | obstacle3 = [21 14; 21 16; 27 16; 27 14]; 7 | obstacle4 = [20 24; 22 24; 24 26; 26 24; 24 28]; 8 | 9 | galleryMap = [boundary; boundary(1,:); NaN NaN; 10 | obstacle1; obstacle1(1,:); NaN NaN; 11 | obstacle2; obstacle2(1,:); NaN NaN; 12 | obstacle3; obstacle3(1,:); NaN NaN; 13 | obstacle4; obstacle4(1,:); NaN NaN]; 14 | 15 | tour = [4 28; 5 3; 27 27; 26 5]; 16 | 17 | figure(1); clf; hold on; 18 | plot(galleryMap(:,1),galleryMap(:,2)); 19 | plot(tour(:,1), tour(:,2), 'rx'); 20 | title('Gallery Tour'); 21 | axis([-1 31 -1 31]) 22 | 23 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-GalleryPField/gallerywavefront/gallerywavefront.m: -------------------------------------------------------------------------------- 1 | %% Wavefront planning in gallery 2 | gallery; 3 | 4 | obs = [obstacle1; obstacle1(1,:); NaN NaN; 5 | obstacle2; obstacle2(1,:); NaN NaN; 6 | obstacle3; obstacle3(1,:); NaN NaN; 7 | obstacle4; obstacle4(1,:); NaN NaN]; 8 | 9 | % Convert map to occupancy grid 10 | M = max(boundary(:,1)); 11 | N = max(boundary(:,2)); 12 | map = ones(M,N); 13 | 14 | for i=1:M 15 | for j = 1:N 16 | if (inpolygon(i,j,boundary(:,1), boundary(:,2))) 17 | map(i,j) = inpolygon(i,j,obs(:,1), obs(:,2)); 18 | end 19 | end 20 | end 21 | 22 | %% Set up simulation 23 | 24 | Tmax = 200; 25 | startPos = tour(1,:); 26 | endCount = length(tour(:,1)); 27 | curGoal = 1; 28 | x = zeros(2,Tmax); 29 | x(:,1) = startPos'; 30 | dx = 0.01; 31 | t = 1; 32 | gVcur = [1 1]; 33 | newplan = 1; 34 | t_start = 1; 35 | 36 | while ((t pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((r=M||iz<=1||iz>=N) 27 | meas_r(i) = rmax; % alpha*j; 28 | break; 29 | % If in the map but hitting an obstacle, set measurement range and 30 | % stop going further 31 | elseif (map(ix,iz)) 32 | meas_r(i) = alpha*j; 33 | break; 34 | end 35 | end 36 | end 37 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-10-VerticalMapping/inversescanner.m: -------------------------------------------------------------------------------- 1 | function [m] = inversescanner(M,N,x,z,theta,meas_phi,meas_r,rmax,alpha,beta) 2 | % Calculates the inverse measurement model for a laser scanner 3 | % Identifies three regions, the first where no new information is 4 | % available, the second where objects are likely to exist and the third 5 | % where objects are unlikely to exist 6 | 7 | % Range finder inverse measurement model 8 | for i = 1:M 9 | for j = 1:N 10 | % Find range and bearing to the current cell 11 | r = sqrt((i*alpha-x)^2+(j*alpha-z)^2); 12 | phi = mod(atan2(j*alpha-z,i*alpha-x)-theta,2*pi); 13 | 14 | % Find the applicable range measurement 15 | [meas_cur,k] = min(abs(phi-meas_phi)); 16 | phi_s(i,j) = phi; 17 | 18 | % If behind out of range measurement, or outside of field 19 | % of view, no new information is available 20 | if ((meas_r(k) == rmax) && (r - rmax >= -alpha/2)) || (abs(phi-meas_phi(k))>beta/2) 21 | m(i,j) = 0.5; 22 | % If the range measurement was before this cell, likely to be an object 23 | elseif ((r - meas_r(k) >= -alpha/2)) 24 | m(i,j) = 0.7 - 0.2*(1-exp(-(r-meas_r(k)))); 25 | % If the cell is in front of the range measurement, likely to be 26 | % empty 27 | else 28 | m(i,j) = 0.3; 29 | end 30 | % Solid ground under robot 31 | %if (abs(phi+theta-3*pi/2) <= beta/2) && (r >= 1) 32 | % m(i,j) = 0.9; 33 | %end 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-Ellipse/ellipse.m: -------------------------------------------------------------------------------- 1 | % Example of error ellipse and Gaussian noise generation 2 | 3 | % Define distribution 4 | mu = [1; 2]; 5 | S = [4 -1; -1 1]; 6 | 7 | % Find eigenstuff 8 | [SE, Se]=eig(S); 9 | 10 | % Generate samples 11 | samples = SE*sqrt(Se)*randn(2,10000); 12 | 13 | % Create ellipse plots 14 | figure(1);clf;hold on; 15 | %error_ellipse(S,mu,0.5) 16 | %error_ellipse(S,mu,0.99) 17 | plot(mu(1) + samples(1,:),mu(2) + samples(2,:), 'r.') 18 | axis equal -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/EKF.m: -------------------------------------------------------------------------------- 1 | function [mu,S,K,mup] = EKF(g,Gt,Ht,S,Y,sensor_model,R,Q) 2 | %% Extended Kalman Filter. 3 | %This function takes Covariance, motion and sensor model and their derivaties and noise as input 4 | %and returns Kalman Gain, Updated mu & Covariance and predicted mu. 5 | 6 | %% Extended Kalman Filter Estimation 7 | % Linearization 8 | % taking linearized models as input so as to make this function more generic 9 | 10 | % Prediction 11 | mup = g; 12 | n=length(mup); 13 | Sp = Gt*S*Gt' + R; 14 | 15 | % update 16 | K = Sp*Ht'*inv(Ht*Sp*Ht'+Q); 17 | mu = mup + K*(Y-sensor_model(mup)); 18 | S = (eye(n)-K*Ht)*Sp; 19 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/MultiRateEKFGit.m: -------------------------------------------------------------------------------- 1 | function [Xk,Sk]=MultiRateEKFGit(Y,V,f,e,X0,R,Q,Sk,n,Cm) 2 | 3 | dt = 1/f; 4 | 5 | 6 | 7 | 8 | % Predict state 9 | 10 | A=LinearOmniMotion(Y,V,X0(3),dt); 11 | S=A*Sk*A'+R; 12 | % Measurement 13 | [Z,h,H]=omniLinearMeasure(X0,Q); 14 | 15 | 16 | % Update State 17 | 18 | % Correction 19 | d=size(Cm); 20 | for j=1:d(2); 21 | if(mod(n,Cm(1,j))==0) 22 | Q1=Q; 23 | Q1(j,j)=Cm(2,j); 24 | [Zt,ht,H]=omniLinearMeasure(X0,Q1); 25 | Z(j,1)=Zt(j,1); 26 | end 27 | end 28 | 29 | % Kalman gain 30 | K= S*H'*inv(H*S*H'+Q); 31 | 32 | % Generate measurement 33 | Xk=X0+K*(Z-h); 34 | Sk=(eye(3)-K*H)*S; 35 | 36 | 37 | 38 | end 39 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/MultiRateEKFGit.m~: -------------------------------------------------------------------------------- 1 | function MultiRateEKFGit(W,f,r,l,e,X0,R,Q,Sk,n,Cm) 2 | 3 | dt = 1/f; 4 | 5 | %initial state 6 | X(:,1)=X0; 7 | Xk(:,1)=X0; 8 | 9 | 10 | for i=2:n*f 11 | 12 | % World frame motion 13 | [Xideal(:,i),X(:,i),Y,V]=omniRobotFrame(r,l,W,X0(3),dt); 14 | 15 | Xideal(:,i)=X0+V; 16 | N=Noise(3,e) 17 | X(:,i)=Xideal(:,i)+N; 18 | 19 | % Update initial state 20 | X0=Xideal(:,i); 21 | 22 | 23 | 24 | % Predict state 25 | 26 | A=LinearOmniMotion(r,l,W,X0(3),dt); 27 | S=A*Sk*A'+R; 28 | % Measurement 29 | [Z,h,H]=omniLinearMeasure(X(:,i-1),Q); 30 | 31 | 32 | % Update State 33 | 34 | % Correction 35 | d=size(Cm); 36 | for j=1:d(2); 37 | if(mod(i,Cm(1,j))==0) 38 | Q1=Q; 39 | Q1(j,j)=Cm(2,j); 40 | [Zt,ht,H]=omniLinearMeasure(X(:,i-1),Q1); 41 | Z(j,1)=Zt(j,1); 42 | end 43 | end 44 | 45 | % Kalman gain 46 | K= S*H'*inv(H*S*H'+Q); 47 | 48 | % Generate measurement 49 | Xk(:,i)=X0(:,i-1)+K*(Z-h); 50 | Sk=(eye(3)-K*H)*S; 51 | 52 | end 53 | 54 | figure 55 | plot(Xideal(1,:),Xideal(2,:),'b-',X(1,:),X(2,:),'r-',Xk(1,:),Xk(2,:),'g-'); 56 | title('Ideal,True and Estimated position Multi rate EKF'); 57 | 58 | 59 | end 60 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/Noise.m: -------------------------------------------------------------------------------- 1 | function N=Noise(n,e) 2 | 3 | N=randn(n,1); 4 | N(3)=toRad(N(n)); 5 | N=e*N; 6 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omniRobotFrame.m: -------------------------------------------------------------------------------- 1 | function [Xideal,X,Y,V]=omniRobotFrame(r,l,W,Xinit,dt,e) 2 | 3 | Y=[ 0 -r*sqrt(3)/2 r*sqrt(3)/2 ;... 4 | r -r/2 -r/2;... 5 | r/(3*l) r/(3*l) r/(3*l);]*W; 6 | Wf=worldFrame(Xinit(3)); 7 | V=Wf*[sqrt(Y(1)^2+Y(2)^2)*dt sqrt(Y(1)^2+Y(2)^2)*dt Y(3)*dt]'; 8 | Xideal=Xinit+V; 9 | N=Noise(3,e) 10 | X=Xideal+N; 11 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omniRobotFrame.m~: -------------------------------------------------------------------------------- 1 | function [Xideal,X,Y,V]=omniRobotFrame(r,l,W,X0,dt,e) 2 | 3 | Y=[ 0 -r*sqrt(3)/2 r*sqrt(3)/2 ;... 4 | r -r/2 -r/2;... 5 | r/(3*l) r/(3*l) r/(3*l);]*W; 6 | Wf=worldFrame(th); 7 | V=Wf*[sqrt(Y(1)^2+Y(2)^2)*dt sqrt(Y(1)^2+Y(2)^2)*dt Y(3)*dt]'; 8 | Xideal=X0+V; 9 | N=Noise(3,e) 10 | X=Xideal(:,i)+N; 11 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omnibot_linearize_motion_model.m: -------------------------------------------------------------------------------- 1 | function Gt = omnibot_linearize_motion_model(mu,v,dt) 2 | Gt=[1 0 -v*sin(mu(3))*dt; 0 1 v*cos(mu(3))*dt; 0 0 1]; 3 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omnibot_linearize_sensor_model.m: -------------------------------------------------------------------------------- 1 | function Ht = omnibot_linearize_sensor_model(mu) 2 | n = length(mu); 3 | Ht = eye(n); 4 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omnibot_motion_model.m: -------------------------------------------------------------------------------- 1 | function mup = omnibot_motion_model(mu,w,v,dt) 2 | 3 | mup=[mu(1)+v*cos(mu(3))*dt; mu(2)+v*sin(mu(3))*dt; mu(3)+w*dt]; 4 | 5 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/omnibot_sensor_model.m: -------------------------------------------------------------------------------- 1 | function y = omnibot_sensor_model(mu) 2 | 3 | y=[mu(1); mu(2); mu(3)]; 4 | 5 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/toRad.m: -------------------------------------------------------------------------------- 1 | function rad=toRad(x) 2 | 3 | rad=x*pi/180; 4 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ExtendedKalmanFilter/worldFrame.m: -------------------------------------------------------------------------------- 1 | function Wf=worldFrame(th) 2 | 3 | Wf=[cos(th) 0 0; 0 sin(th) 0; 0 0 1;]; 4 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ParticleFilter/ellipse.m: -------------------------------------------------------------------------------- 1 | % Example of error ellipse and Gaussian noise generation 2 | 3 | % Define distribution 4 | mu = [1; 2]; 5 | S = [4 -1; -1 1]; 6 | 7 | % Find eigenstuff 8 | [SE, Se]=eig(S); 9 | 10 | % Generate samples 11 | samples = SE*sqrt(Se)*randn(2,10000); 12 | 13 | % Create ellipse plots 14 | figure(1);clf;hold on; 15 | %error_ellipse(S,mu,0.5) 16 | %error_ellipse(S,mu,0.99) 17 | plot(mu(1) + samples(1,:),mu(2) + samples(2,:), 'r.') 18 | axis equal -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-5-ParticleFilter/expectation.m: -------------------------------------------------------------------------------- 1 | %Expectation 2 | clear; clc; 3 | % Distribution 4 | L=3; 5 | mu1 = 0; % mean (mu) 6 | S1 = .3;% std deviation 7 | x = [mu1-L*sqrt(S1):0.01:mu1+L*sqrt(S1)]; 8 | xmin = min(x); xmax = max(x); 9 | fx = normpdf(x,mu1,S1); % p(x) 10 | Fx = cumsum(fx); 11 | 12 | A = [-0.5 0.2]; 13 | 14 | I = zeros(1,length(x)) 15 | ind1 = find(x>A(1),1) 16 | ind2 = find(xseed,1),:,t); 12 | end 13 | 14 | 15 | end 16 | 17 | 18 | figure(1); clf; 19 | subplot(3,3,1) 20 | plot(X(:,1,1),X(:,2,1),'ro') 21 | axis([0 1 0 1]); 22 | xlabel('t=1') 23 | subplot(3,3,2) 24 | plot(X(:,1,2),X(:,2,2),'ro') 25 | axis([0 1 0 1]); 26 | xlabel('t=2') 27 | subplot(3,3,3) 28 | plot(X(:,1,5),X(:,2,5),'ro') 29 | axis([0 1 0 1]); 30 | xlabel('t=5') 31 | subplot(3,3,4) 32 | plot(X(:,1,10),X(:,2,10),'ro') 33 | axis([0 1 0 1]); 34 | xlabel('t=10') 35 | subplot(3,3,5) 36 | plot(X(:,1,20),X(:,2,20),'ro') 37 | axis([0 1 0 1]); 38 | xlabel('t=20') 39 | subplot(3,3,6) 40 | plot(X(:,1,30),X(:,2,30),'ro') 41 | axis([0 1 0 1]); 42 | xlabel('t=30') 43 | subplot(3,3,7) 44 | plot(X(:,1,50),X(:,2,50),'ro') 45 | axis([0 1 0 1]); 46 | xlabel('t=50') 47 | subplot(3,3,8) 48 | plot(X(:,1,70),X(:,2,70),'ro') 49 | axis([0 1 0 1]); 50 | xlabel('t=70') 51 | subplot(3,3,9) 52 | plot(X(:,1,200),X(:,2,200),'ro') 53 | axis([0 1 0 1]); 54 | xlabel('t=200') 55 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-6-EKFLocalization/circle.m: -------------------------------------------------------------------------------- 1 | function circle(fig,pos,r) 2 | % circle(fig, pos, r) 3 | % args: figure number, position in x,y, radius r 4 | figure(fig); 5 | rectangle('Position',[pos(1)-r,pos(2)-r,2*r,2*r], 'Curvature', [1 1], 'LineWidth',1, 'EdgeColor','b'); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-6-EKFLocalization/inview.m: -------------------------------------------------------------------------------- 1 | function yes = inview(f,x, rmax, thmax) 2 | % Checks if a feature is in view 3 | yes = 0; 4 | 5 | dx = f(1)-x(1); 6 | dy = f(2)-x(2); 7 | 8 | r = sqrt(dx^2+dy^2); 9 | th = mod(atan2(dy,dx)-x(3),2*pi); 10 | if (th > pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((r pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((r pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((r pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((r pi) 11 | th = th-2*pi; 12 | end 13 | 14 | if ((rabs(dx); 22 | if steep t=dx;dx=dy;dy=t; end 23 | 24 | %The main algorithm goes here. 25 | if dy==0 26 | q=zeros(dx+1,1); 27 | else 28 | q=[0;diff(mod([floor(dx/2):-dy:-dy*dx+floor(dx/2)]',dx))>=0]; 29 | end 30 | 31 | %and ends here. 32 | 33 | if steep 34 | if y1<=y2 y=[y1:y2]'; else y=[y1:-1:y2]'; end 35 | if x1<=x2 x=x1+cumsum(q);else x=x1-cumsum(q); end 36 | else 37 | if x1<=x2 x=[x1:x2]'; else x=[x1:-1:x2]'; end 38 | if y1<=y2 y=y1+cumsum(q);else y=y1-cumsum(q); end 39 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-6-ScanRegistration/funnymap.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/code v1.0/ME597-6-ScanRegistration/funnymap.jpg -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-6-ScanRegistration/getranges.m: -------------------------------------------------------------------------------- 1 | function meas_r = getranges(map,X,meas_phi, rmax) 2 | % Generate range measurements for a laser scanner based on a map, vehicle 3 | % position and sensor parameters. 4 | % Rough implementation of ray tracing algorithm. 5 | 6 | % Initialization 7 | [M,N] = size(map); 8 | x = X(1); 9 | y = X(2); 10 | th = X(3); 11 | meas_r = rmax*ones(size(meas_phi')); 12 | 13 | % For each measurement bearing 14 | for i=1:length(meas_phi) 15 | % For each unit step along that bearing up to max range 16 | for r=1:rmax 17 | % Determine the coordinates of the cell 18 | xi = round(x+r*cos(th+meas_phi(i))); 19 | yi = round(y+r*sin(th+meas_phi(i))); 20 | % If not in the map, set measurement there and stop going further 21 | if (xi<=1||xi>=M||yi<=1||yi>=N) 22 | meas_r(i) = sqrt((x-xi)^2+(y-yi)^2); 23 | break; 24 | % If in the map but hitting an obstacle, set measurement range and 25 | % stop going further 26 | elseif (map(xi,yi)) 27 | meas_r(i) = sqrt((x-xi)^2+(y-yi)^2); 28 | break; 29 | end 30 | end 31 | end 32 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-6-ScanRegistration/inversescannerbres.m: -------------------------------------------------------------------------------- 1 | function [m] = inversescannerbres(M,N,x,y,theta,r,rmax) 2 | % Calculates the inverse measurement model for a laser scanner through 3 | % raytracing with Bresenham's algorithm, assigns low probability of object 4 | % on ray, high probability at end. Returns cells and probabilities. 5 | m = []; 6 | % Range finder inverse measurement model 7 | x1 = max(1,min(M,round(x))); 8 | y1 = max(1,min(N,round(y))); 9 | 10 | endpt = [x y] + r*[cos(theta) sin(theta)]; 11 | 12 | x2 = max(1,min(M,round(endpt(1)))); 13 | y2 = max(1,min(N,round(endpt(2)))); 14 | 15 | [list(:,1) list(:,2)] = bresenham(x1,y1,x2,y2); 16 | 17 | m = [list 0.4*ones(length(list(:,1)),1)]; 18 | if (r7 5 | disp('Please choose a proper input trajectory (1-7).') 6 | disp('What is the input trajectory?') 7 | disp('1: Spiral; 2: Nudges; 3: Swerves; 4: Corner;') 8 | disp('5: A single lane change') 9 | disp('6: Get around an obstacle') 10 | disp('7: User defined motion model') 11 | prompt = ''; 12 | index = input(prompt); 13 | end 14 | end 15 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/lane_change.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=lane_change(T, dt, xd, xddot) 2 | % Define the input velocity 3 | v = 2*ones(size(T)); 4 | % Define the input angular velocity 5 | w = zeros(size(T)); 6 | c = floor(length(w)/8); 7 | w(2*c+1:3*c) = 1; 8 | w(3*c+1:4*c) = -1; 9 | 10 | [xddot, xd]=motion_model(T, dt, xd, xddot, v, w); 11 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/motion_model.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=motion_model(T, dt, xd, xddot, v, w) 2 | % Define the states 3 | % xd1: x-position; xd2: y-position; xd3: heading angle 4 | % xddot1: x-velocity; xddot2: y-velocity; xddot3: angular velocity 5 | for t=1:length(T) 6 | xddot(:,t) = [v(t)*cos(xd(3,t)); 7 | v(t)*sin(xd(3,t)); 8 | w(t)]; 9 | xd(:,t+1) = xd(:,t)+dt*xddot(:,t); 10 | end 11 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/nudges.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=nudges(T, dt, xd, xddot, i) 2 | % Define the input velocity 3 | v = 2*ones(size(T)); 4 | % Define the input angular velocity 5 | w = zeros(size(T)); 6 | c = floor(length(w)/8); 7 | w(2*c+1) = (-3+i)/0.2; 8 | 9 | [xddot, xd]=motion_model(T, dt, xd, xddot, v, w); 10 | end 11 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/obstacle.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=obstacle(T, dt, xd, xddot) 2 | % Define the input velocity 3 | v = 2*ones(size(T)); 4 | % Define the input angular velocity 5 | w = zeros(size(T)); 6 | c = floor(length(w)/8); 7 | w(2*c+1:3*c) = 1/3; 8 | w(3*c+1:4*c) = -1/3; 9 | w(4*c+1:5*c) = -1/3; 10 | w(5*c+1:6*c) = 1/3; 11 | 12 | [xddot, xd]=motion_model(T, dt, xd, xddot, v, w); 13 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/plot_figure.m: -------------------------------------------------------------------------------- 1 | function plot_figure(xd, T, index) 2 | figure(index); 3 | hold on; 4 | 5 | % Select proper scale size to plot 6 | switch index 7 | case 1 8 | scale = 0.05; 9 | title('Desired Trajectory: Spiral'); 10 | case 2 11 | scale = .3; 12 | title('Desired Trajectory: Nudges'); 13 | case 3 14 | scale = .3; 15 | title('Desired Trajectory: Swerves'); 16 | case 4 17 | scale = .3; 18 | title('Desired Trajectory: Corner'); 19 | case 5 20 | scale = .3; 21 | title('Desired Trajectory: A single lane change'); 22 | case 6 23 | scale = .3; 24 | title('Desired Trajectory: Get around an obstacle'); 25 | % Plot the obstacle 26 | ang=0:0.01:2*pi; 27 | xp=1.5*cos(ang); 28 | yp=1.5*sin(ang); 29 | plot(7+xp,8.5+yp); 30 | hold on; 31 | case 7 32 | scale = 0.05; 33 | title('Desired Trajectory: User defined trajectory'); 34 | end 35 | 36 | plot(xd(1,:),xd(2,:)); 37 | % Plot a two-wheel robot along the trajectory 38 | for t=1:5:length(T) 39 | drawcar(xd(1,t),xd(2,t),xd(3,t),scale,index); 40 | end 41 | 42 | axis equal; 43 | end 44 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/spiral.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=spiral(T, dt, xd, xddot) 2 | % Define the input velocity 3 | v = sin(0.2*T); 4 | % Define the input angular velocity 5 | w = ones(size(T)); 6 | 7 | [xddot, xd]=motion_model(T, dt, xd, xddot, v, w); 8 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/swerves.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=swerves(T, dt, xd, xddot) 2 | % Define the input velocity 3 | v = 2*ones(size(T)); 4 | % Define the input angular velocity 5 | w = zeros(size(T)); 6 | c = floor(length(w)/8); 7 | w(2*c+1:3*c) = 1; 8 | w(3*c+1:4*c) = -1; 9 | 10 | [xddot, xd]=motion_model(T, dt, xd, xddot, v, w); 11 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-7-Trajectories/user_define_motion_model.m: -------------------------------------------------------------------------------- 1 | function [xddot, xd]=user_define_motion_model(T, dt, xd, xddot) 2 | % Define the input velocity 3 | v = sin(0.2*T); 4 | % Define the input angular velocity 5 | w = ones(size(T)); 6 | 7 | for t=1:length(T) 8 | xddot(:,t) = [v(t)*cos(xd(3,t)); 9 | v(t)*sin(xd(3,t)); 10 | w(t)]; 11 | xd(:,t+1) = xd(:,t)+dt*xddot(:,t); 12 | end 13 | end 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/lpsolve55.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/code v1.0/ME597-8-MILP/lpsolve55.dll -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/mxlpsolve.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WaterlooRobotics/mobilerobotics/5c4f42501956bcd9ebd926a7bc5d899f02b5d70a/matlab_simulation/code v1.0/ME597-8-MILP/mxlpsolve.mexw64 -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'facecolor','none','linestyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-MILP/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PRM/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PRM/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PRM/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PRM/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'facecolor','none','linestyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PRM/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/distToEdge.m: -------------------------------------------------------------------------------- 1 | function [d,pt] = distToEdge(v,edge) 2 | % Computes the shortest distance to a line segment, returns distance and 3 | % closest point 4 | 5 | S = edge(3:4) - edge(1:2); 6 | S1 = v - edge(1:2); 7 | m1 = S1*S'; 8 | if (m1 <= 0 ) 9 | d = norm(v-edge(1:2)); 10 | pt = edge(1:2); 11 | else 12 | m2 = S*S'; 13 | if ( m2 <= m1 ) 14 | d = norm(v-edge(3:4)); 15 | pt = edge(3:4); 16 | else 17 | b = m1 / m2; 18 | pt = edge(1:2) + b*S; 19 | d = norm(v-pt); 20 | end 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/minDistToEdges.m: -------------------------------------------------------------------------------- 1 | function [minD,minPt, d, pt, ind] = minDistToEdges(v,edges, fig) 2 | % Computes the shortest distance to a sequence of edges, which may form a 3 | % path, for example. Returns the min distance, the closest point, and the 4 | % list of distances and closest points for each edge 5 | if (nargin < 3) 6 | fig = 0; 7 | end 8 | 9 | 10 | n = length(edges(:,1)); 11 | 12 | for i=1:n 13 | [d(i), pt(i,:)] = distToEdge(v,edges(i,:)); 14 | end 15 | 16 | [minD, ind] = min(d); 17 | minPt = pt(ind,:); 18 | 19 | if (fig) 20 | figure(fig); hold on; 21 | l = [v; minPt]; 22 | plot(l(:,1),l(:,2),'g'); 23 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','FaceColor',[1 0 0],'EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'FaceColor','none','LineStyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-PotentialFields/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/Trapezoid.m: -------------------------------------------------------------------------------- 1 | %% Visibility graph 2 | clear all; close all; clc 3 | 4 | % Set up environment 5 | posMinBound = [0 0]; 6 | posMaxBound = [12 10]; 7 | numObsts = 5; 8 | endPos = [1 1]; 9 | startPos = [21.5 18.5]; 10 | 11 | minLen.a = 1; 12 | maxLen.a = 3; 13 | minLen.b = 2; 14 | maxLen.b = 6; 15 | 16 | obstBuffer = 0.5; 17 | maxCount = 10000; 18 | 19 | [aObsts,bObsts,obsPtsStore] = polygonal_world(posMinBound, posMaxBound, minLen, maxLen, numObsts, startPos, endPos, obstBuffer, maxCount); 20 | 21 | figure(1); clf; 22 | hold on; 23 | plotEnvironment(obsPtsStore,posMinBound, posMaxBound, startPos, endPos); 24 | 25 | traps = trapezoidalDecomposition(aObsts,bObsts,obsPtsStore,posMinBound,posMaxBound); 26 | plotTrapezoids(traps); 27 | 28 | for i=1:length(traps) 29 | nodes(i,:) = (traps(i).pts(1,:)+traps(i).pts(3,:))/2; 30 | end 31 | for i=1:length(traps) 32 | for j=1:length(traps) 33 | if ((norm(traps(i).pts(2,:)-traps(j).pts(1,:))<0.01) || ... 34 | (norm(traps(i).pts(2,:)-traps(j).pts(4,:))<0.01) || ... 35 | (norm(traps(i).pts(3,:)-traps(j).pts(1,:))<0.01) || ... 36 | (norm(traps(i).pts(3,:)-traps(j).pts(4,:))<0.01)) 37 | A(i,j) = 1; 38 | figure(1);hold on; 39 | plot([nodes(i,1) nodes(j,1)],[nodes(i,2) nodes(j,2)]) 40 | end 41 | end 42 | end 43 | 44 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/calculatePolyPoints.m: -------------------------------------------------------------------------------- 1 | function polygons = calculatePolyPoints(polygons) 2 | %calculates the points of a polygon. It assumes that the edges are in 3 | %order (meaning if start clockwise or counterclockwise the edges are in 4 | %order that you would reach them) 5 | 6 | for i = 1:1:length(polygons) 7 | if(~polygons(i).valid) 8 | continue; 9 | end 10 | 11 | numEdges = size(polygons(i).a,1); 12 | nextEdge = [2:1:numEdges, 1]; 13 | for j = 1:1:numEdges 14 | 15 | m = nextEdge(j); 16 | % if j == numEdges 17 | % m = 1; 18 | % else 19 | % m = j + 1; 20 | % end 21 | 22 | pt = [-(polygons(i).a(m,2)*polygons(i).b(j) - polygons(i).b(m)*polygons(i).a(j,2))/(polygons(i).a(m,1)*polygons(i).a(j,2) - polygons(i).a(j,1)*polygons(i).a(m,2));... 23 | (polygons(i).a(m,1)*polygons(i).b(j) - polygons(i).a(j,1)*polygons(i).b(m))/(polygons(i).a(m,1)*polygons(i).a(j,2) - polygons(i).a(m,2)*polygons(i).a(j,1))]; 24 | 25 | polygons(i).pts(j,:) = pt; 26 | end 27 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/closeTraps.m: -------------------------------------------------------------------------------- 1 | % iterates over all of the trapezoids that are not completed, and figures 2 | % out if the point passed in is inside the trapezoid. If so, it completes 3 | % the trapezoid and closes it off. Assumption that the last edge is 4 | % vertical. (vertical decomposition). 5 | 6 | function traps = closeTraps(traps, point) 7 | 8 | for j = 1:1:length(traps) 9 | if(~traps(j).valid) 10 | count = 0; 11 | for k = 1:1:traps(j).numEdges 12 | if( traps(j).a(k,:)*point - traps(j).b(k) <= 100*eps) 13 | count = count + 1; 14 | end 15 | end 16 | 17 | %the point is inside the trapezoid 18 | if(count == size(traps(j).a,1)-1) 19 | traps(j).a(end,:) = [1;0]; 20 | traps(j).b(end) = traps(j).a(end,:)*point; 21 | traps(j).valid = 1; 22 | traps(j).numEdges = traps(j).numEdges + 1; 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'facecolor','none','linestyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/plotTrapezoids.m: -------------------------------------------------------------------------------- 1 | %this function will plot a list of trapezoids and label them with their index. 2 | function plotTrapezoids(traps) 3 | 4 | colours = [0.8 1 0.8;1 1 0.8;0.8 0.8 1 ]; 5 | hold on 6 | for m = 1:1:length(traps) 7 | if(~traps(m).valid) 8 | continue; 9 | end 10 | patch(traps(m).pts(:,1),traps(m).pts(:,2), colours(mod(m,3)+1,:) ,'linestyle','-','EdgeColor',[0.3 0.6 0.3]);%,'facecolor','none' 11 | %text(mean(traps(m).pts(:,1)), mean(traps(m).pts(:,2)),sprintf('%d',m)); 12 | end 13 | hold off 14 | % axis equal 15 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Trapezoid/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Visibility/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Visibility/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Visibility/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Visibility/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'facecolor','none','linestyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Visibility/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/IKOmniDirectionRobot.m: -------------------------------------------------------------------------------- 1 | function [ angular_wheel_velocity ] = IKOmniDirectionRobot( world_velocity, l, r) 2 | %IKOMNIDIRECTIONROBOT Calculates the wheel angular velocities based on the 3 | %required world referenced robot velocity. Assumes 4 | % The robot has wheel 1 parallel to the horizontal world axis (theta=0) 5 | 6 | theta = 0; 7 | world_velocity = [world_velocity 0]; 8 | rot = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]; 9 | robot_velocity = rot*transpose(world_velocity); 10 | wheel_velocity = [-sin(pi/3) cos(pi/3) l; 0 -1 l; sin(pi/3) cos(pi/3) l] * robot_velocity; 11 | angular_wheel_velocity = wheel_velocity / r; 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/createVoronoiGraph.m: -------------------------------------------------------------------------------- 1 | function [ G ] = createVoronoiGraph( C, V ) 2 | %CREATEVORONOIGRAPH Creates a Graph object our of the Voronoi diagram 3 | % For each set of Voronoi vertices it adds them and th edge they 4 | % represent to the the graph object 5 | 6 | G = graph; 7 | G = addnode(G,length(V)); 8 | 9 | for i=1:length(C) 10 | for k=1:(length(C{i})-1) 11 | idxOut = findedge(G,C{i}(k),C{i}(k+1)); 12 | if(idxOut == 0) 13 | G = addedge(G, C{i}(k), C{i}(k+1), distanceTwoPoints(V(C{i}(k),:),V(C{i}(k+1),:))); 14 | end; 15 | end; 16 | end; 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/distanceClosestPointToLine.m: -------------------------------------------------------------------------------- 1 | function [ distToLine ] = distanceClosestPointToLine( points, endLine1, endLine2 ) 2 | %DISTANCEPOINTTOLINE Finds the distance from the closest obstacle for a Voronoiedge 3 | % A midpoint along the voronoi edge is calculated. From the array of 4 | % obstacles, the closest to the midpoint is found. This will also be one 5 | % of the two points that correspond to the Voronoi edge. The distance 6 | % from the obstacle to the Voronoi edge is calculated, to make sure the 7 | % robot can fit through. 8 | midpoint = [endLine1(1)+(endLine2(1)-endLine1(1))/2 endLine1(2)+(endLine2(2)-endLine1(2))/2]; 9 | K = dsearchn(points, midpoint); 10 | 11 | a = [(endLine1 - endLine2), 0]; 12 | b = [(points(K,:) - endLine2), 0]; 13 | distToLine = norm(cross(a,b))/norm(a); 14 | end 15 | 16 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/distanceTwoPoints.m: -------------------------------------------------------------------------------- 1 | function [ dist ] = distanceTwoPoints( point1, point2 ) 2 | %DISTANCETWOPOINTS Returns the distance between two points 3 | % Detailed explanation goes here 4 | if(point1(1) == Inf || point1(2) == Inf || point1(1) == Inf || point2(2) == Inf) 5 | dist = Inf; 6 | else 7 | dist = sqrt((point1(1)-point2(1))^2 + (point1(2)-point2(2))^2); 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/erasePath.m: -------------------------------------------------------------------------------- 1 | function [ ] = erasePath( pathLineObjects ) 2 | %ERASEPATH Erases already drawn path line objects 3 | for i = 1:length(pathLineObjects) 4 | delete(pathLineObjects(i)); 5 | end 6 | 7 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/getVelocityVectors.m: -------------------------------------------------------------------------------- 1 | function [ wheelVelocity ] = getVelocityVectors( path, V, l, r ) 2 | %GETVELOCITYVECTORS Obtains the wheel velocity vectors for an omniwheel robot. 3 | 4 | 5 | i=1; 6 | wheelVelocity = []; 7 | while i<=(length(path)-1) 8 | worldVelocity = V(path(i+1),:) - V(path(i),:); 9 | wheelVelocity = [wheelVelocity; transpose(IKOmniDirectionRobot(worldVelocity, l, r))]; 10 | i = i+1; 11 | end; 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Voronoi/outOfBounds.m: -------------------------------------------------------------------------------- 1 | function [ invalid ] = outOfBounds( point, upperBound ) 2 | %OUTOFBOUNDS Determines if a point is out of the space bounds 3 | % This is required since some of the Voronoi vertices are outside the 4 | % space area, and paths through these vertices should not be considered. 5 | % In other words, you can imagine a wall surrounding the playing space. 6 | if (point(1,1)<0 || point(1,2)<0 || point(1,1)>upperBound || point(1,2)>upperBound) 7 | invalid = 1; 8 | else invalid = 0; 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/AffineIntersect.m: -------------------------------------------------------------------------------- 1 | function [ pt, doesIntersect ] = AffineIntersect( edge1, edge2 ) 2 | %AFFINEINTERSECT Finds interesection between two lines 3 | % Params: 4 | % edge1 -> First edge to be tested (struct form edge1.A, edge1.b) 5 | % edge2 -> Second edge to be tested (struct form edge2.A, edge2.b) 6 | % Returns: 7 | % pt -> The interesection point (of form [x y]) 8 | % doesIntersect -> Returns 1 if the points do in fact intersect 9 | 10 | A = [edge1.A ; edge2.A] ; 11 | b = [edge1.b ; edge2.b] ; 12 | 13 | A = min(A,1E6); 14 | 15 | if (cond(A) > 1E6) 16 | doesIntersect = 0 ; 17 | pt = [0 0] ; 18 | return ; 19 | end 20 | 21 | 22 | pt = (inv(A) * b)' ; 23 | doesIntersect = 1 ; -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/CheckCollision.m: -------------------------------------------------------------------------------- 1 | function [ inCollision, edge ] = CheckCollision( ptA, ptB, obstEdges ) 2 | %CHECKCOLLISION Checks if a line interesects with a set of edges 3 | % Detailed explanation goes here 4 | 5 | % Check for each edge 6 | edge = []; 7 | for k = 1:size(obstEdges,1) 8 | % If both vertices aren't to the left, right, above, or below the 9 | % edge's vertices in question, then test for collision 10 | if ~((max([ptA(1),ptB(1)])max([obstEdges(k,1),obstEdges(k,3)])) || ... 12 | (max([ptA(2),ptB(2)])max([obstEdges(k,2),obstEdges(k,4)]))) 14 | if (EdgeCollision([ptA, ptB], obstEdges(k,:))) 15 | % Eliminate end-to-end contacts from collisions list 16 | if (sum(abs(ptA-obstEdges(k,1:2)))>0 && ... 17 | sum(abs(ptB-obstEdges(k,1:2)))>0 && ... 18 | sum(abs(ptA-obstEdges(k,3:4)))>0 && ... 19 | sum(abs(ptB-obstEdges(k,3:4)))>0) 20 | 21 | edge = k; 22 | inCollision = 1 ; % In Collision 23 | return 24 | end 25 | end 26 | end 27 | end 28 | inCollision = 0 ; % Not in collision -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/EdgePtsToVec.m: -------------------------------------------------------------------------------- 1 | function [ A, b ] = EdgePtsToVec( edge ) 2 | %EDGEPTSTOVEC Converts an edge from point notation to vector notation 3 | % edge -> Edge represented by endpoints [x1 y1 x2 y2] 4 | % returns: Line in vector form Ax = b 5 | 6 | f = [edge(1); edge(2); 0 ] ; 7 | g = [edge(3); edge(4); 0 ] ; 8 | 9 | A = cross(f-g, [0;0;1]) ; 10 | A = A' / norm(A) ; 11 | b = A*f ; 12 | A = A(1:2) ; 13 | 14 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/distToEdge.m: -------------------------------------------------------------------------------- 1 | function [d,pt] = distToEdge(v,edge) 2 | % Computes the shortest distance to a line segment, returns distance and 3 | % closest point 4 | 5 | S = edge(3:4) - edge(1:2); 6 | S1 = v - edge(1:2); 7 | m1 = S1*S'; 8 | if (m1 <= 0 ) 9 | d = norm(v-edge(1:2)); 10 | pt = edge(1:2); 11 | else 12 | m2 = S*S'; 13 | if ( m2 <= m1 ) 14 | d = norm(v-edge(3:4)); 15 | pt = edge(3:4); 16 | else 17 | b = m1 / m2; 18 | pt = edge(1:2) + b*S; 19 | d = norm(v-pt); 20 | end 21 | end 22 | 23 | 24 | -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/minDistToEdges.m: -------------------------------------------------------------------------------- 1 | function [minD,minPt, d, pt, ind] = minDistToEdges(v,edges, fig) 2 | % Computes the shortest distance to a sequence of edges, which may form a 3 | % path, for example. Returns the min distance, the closest point, and the 4 | % list of distances and closest points for each edge 5 | if (nargin < 3) 6 | fig = 0; 7 | end 8 | 9 | 10 | n = length(edges(:,1)); 11 | 12 | for i=1:n 13 | [d(i), pt(i,:)] = distToEdge(v,edges(i,:)); 14 | end 15 | 16 | [minD, ind] = min(d); 17 | minPt = pt(ind,:); 18 | 19 | if (fig) 20 | figure(fig); hold on; 21 | l = [v; minPt]; 22 | plot(l(:,1),l(:,2),'g'); 23 | end -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/plotEnvironment.m: -------------------------------------------------------------------------------- 1 | %takes in the points of the rectangles and plots the environment 2 | 3 | function plotEnvironment(ptsStore,posMinBound, posMaxBound, startPos, endPos) 4 | 5 | hold on 6 | for i = 1:2:size(ptsStore,2) 7 | patch(ptsStore(:,i),ptsStore(:,i+1),[1 0 0],'linestyle','-','FaceColor',[1 0 0],'EdgeColor',[1 0 0], 'LineWidth',2) 8 | end 9 | plot(startPos(1),startPos(2),'b*') 10 | plot(endPos(1),endPos(2),'g*') 11 | patch([posMinBound(1) posMaxBound(1) posMaxBound(1) posMinBound(1)],[posMinBound(2) posMinBound(2) posMaxBound(2) posMaxBound(2)],... 12 | [1 0 0],'FaceColor','none','LineStyle','-','EdgeColor',[0 1 1]) 13 | hold off 14 | axis equal 15 | axis([posMinBound(1) posMaxBound(1) posMinBound(2) posMaxBound(2)]); -------------------------------------------------------------------------------- /matlab_simulation/code v1.0/ME597-8-Wavefront/polygonsOverlap.m: -------------------------------------------------------------------------------- 1 | function val = polygonsOverlap(poly1, poly2) 2 | %POLYGONSOVERLAP Checks if two polygons intersect 3 | % poly1 - N1x2 matrix of x,y coordinates of polygon vertices 4 | % poly2 - N2x2 matrix of x,y coordinates of polygon vertices 5 | % ASSUMES: 6 | % - Polygon vertices are ordered counter clockwise (can be enforced with 7 | % our scripts) 8 | 9 | val = false; 10 | 11 | % Simple test to check if 1 is fully or partially enclosed in polygon 2 12 | if sum(inpolygon(poly1(:,1),poly1(:,2),poly2(:,1),poly2(:,2))) 13 | val = true; 14 | return 15 | end 16 | 17 | % Simple test to check if 2 is fully or partially enclosed in polygon 1 18 | if sum(inpolygon(poly2(:,1),poly2(:,2),poly1(:,1),poly1(:,2))) 19 | val = true; 20 | return 21 | end 22 | 23 | % Close the polygons 24 | poly1 = [poly1;poly1(1,:)]; 25 | obstEdges = [poly2,[poly2(2:end,:);poly2(1,:)]]; 26 | % Loop over all possible intersections 27 | for vertex = 1:(length(poly1)-1) 28 | if (CheckCollision(poly1(vertex,:), poly1(vertex+1,:), obstEdges)) 29 | val = true; 30 | return 31 | end 32 | end -------------------------------------------------------------------------------- /turtlebot/turtlebot_example/launch/amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /turtlebot/turtlebot_example/launch/amcl_live_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /turtlebot/turtlebot_example/launch/gmapping_live_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /turtlebot/turtlebot_example/launch/gmapping_sim_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | --------------------------------------------------------------------------------