clc
clear all
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Create the waypoints with heading_n direction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%set(gcf,'Visible','off')% turns current figure "off"%set(0,'DefaultFigureVisible','off');% all subsequent figures "off"%realVideo();
heading_n=-1;
LostSignal_Button=0;
min_degree =0.5;
arc_length=0.1; a=2.5; b=1.5;
lane_width=1.5;
lane_type=2;if heading_n==1%Clockwise
Left_curb =-call_create_ellipse(lane_type,arc_length,a,b,lane_width);else%Anticlockwise
Left_curb =call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%Find the angles between each waypoint
% yaw=zeros(size(Left_curb,1),1);%%for i=1:size(Left_curb,1)-1%yaw(i,1)=atan2((Left_curb(i+1,2)-(Left_curb(i,2))),(Left_curb(i+1,1))-(Left_curb(i,1)));% end
% i=size(Left_curb,1);%yaw(i,1)=atan2((Left_curb(1,2)-(Left_curb(i,2))),(Left_curb(1,1))-(Left_curb(i,1)));% Left_curb=[Left_curb, yaw*180/pi];%Left_curb=[Left_curb,(1:size(Left_curb,1))'];
lane_type=3;if heading_n==1%Clockwise
Right_curb =-call_create_ellipse(lane_type,arc_length,a,b,lane_width);else%Anticlockwise
Right_curb =call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%Find the angles between each waypoint
% yaw=zeros(size(Right_curb,1),1);%for i=1:size(Right_curb,1)-1%yaw(i,1)=atan2((Right_curb(i+1,2)-(Right_curb(i,2))),(Right_curb(i+1,1))-(Right_curb(i,1)));% end
% i=size(Right_curb,1);%yaw(i,1)=atan2((Right_curb(1,2)-(Right_curb(i,2))),(Right_curb(1,1))-(Right_curb(i,1)));% Right_curb=[Right_curb, yaw*180/pi];%Right_curb=[Right_curb,(1:size(Right_curb,1))'];
lane_type=1;if heading_n==1%Clockwise
Driver_pose =-call_create_ellipse(lane_type,arc_length,a,b,lane_width);else%Anticlockwise
Driver_pose =call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Or ADD your own data points as follows:
x_inner=[-1.9191-1.9419-1.9584-1.9764-1.9905-2.0088-2.0188-2.0352-2.0483-2.0666-2.0702-2.0885-2.0982-2.1099-2.1195-2.1324-2.1397-2.1515-2.1518-2.157-2.1715-2.179-2.1819-2.1867-2.1895-2.1918-2.1954-2.1945-2.1967-2.1952-2.1945-2.1936-2.1907-2.1872-2.1857-2.1794-2.1804-2.1271-2.1244-2.1039-2.0904-2.0708-2.0637-2.0455-2.0232-2.0098-2.0035-1.9802-1.9661-1.921-1.9087-1.8681-1.8404-1.8081-1.7982-1.7772-1.7596-1.7326-1.7147-1.6838-1.6607-1.6339-1.5915-1.5622-1.5625-1.517-1.4942-1.4667-1.4529-1.4279-1.4116-1.3835-1.3602-1.3341-1.3197-1.2942-1.2713-1.2461-1.229-1.202-1.1915-1.1688-1.1537-1.1369-1.1205-1.0986-1.0806-1.0594-1.048-1.0284-1.0221-1.0056-0.995-0.9815-0.9748-0.9618-0.9537-0.9422-0.9363-0.927-0.9228-0.9178-0.8805-0.8642-0.8159-0.7845-0.7495-0.7159-0.6139-0.5911-0.5236-0.4831-0.416-0.3959-0.3767-0.336-0.3044-0.2618-0.2259-0.1883-0.1525-0.1126-0.0878-0.05620.05810.09830.15540.19450.24830.29780.35550.38310.43050.45470.48640.51940.57350.6140.67090.71010.77420.80720.85540.88780.93050.9661.01261.04081.08211.11621.16011.18481.20921.23891.2841.31121.33451.36431.38411.41261.44851.48771.51241.5441.57041.60711.62351.66051.66911.70971.71761.74871.76781.79341.82131.85211.86831.89881.9161.9471.96621.99432.02622.04752.06982.09322.1052.12642.13622.15512.16522.17932.18922.20232.20812.21942.2232.2292.2322.23632.23752.23682.23622.23292.2312.22292.21962.20832.20352.18972.1792.1622.14532.12712.10742.09362.07462.06612.05552.05082.04672.04842.05112.05612.06662.07972.09912.11272.12532.13572.14552.15792.17752.20022.22372.24632.27352.29182.312.33012.34592.36142.37492.39082.39752.40612.41152.41562.41842.422.41942.41592.41572.40222.39372.38062.372.35292.34642.32572.30752.28312.26742.2422.22872.19862.1852.15322.13722.1042.02731.98131.96791.93061.90931.87851.85331.8121.78871.75191.73261.70161.67591.63951.61841.57751.55981.45161.44691.38831.37761.34161.32141.29591.28311.26571.25191.23441.22411.20811.19871.18691.18181.17491.16991.16551.16291.16461.16421.16341.161.15441.14131.13261.08281.05310.99860.96340.91840.89220.84620.80860.76020.71070.64620.60350.47810.45620.4190.37250.36550.32650.32320.30340.29760.2820.2790.26910.26310.25290.24340.23680.22670.2160.20350.19340.18060.16990.1510.13450.10660.08380.0014-0.0285-0.0909-0.13-0.1682-0.2048-0.2465-0.279-0.3252-0.3662-0.4254-0.459-0.5066-0.5483-0.6034-0.6481-0.7048-0.7448-0.8037-0.8475-0.9013-0.9466-0.9996-1.0574-1.1035-1.1518-1.1841-1.2343-1.2651-1.3017-1.3488-1.4013-1.4337-1.4798-1.5163-1.5641-1.6017-1.6434-1.6753-1.7198-1.7828-1.8287-1.8708-1.9091];
y_inner=[1.49941.51481.52671.53971.55091.56521.57351.58831.60151.62051.6261.65181.66431.68481.7011.72521.7431.76771.76441.78671.82071.84821.86371.88841.91.92831.93771.96321.98282.0092.042.06832.07672.10072.12032.15282.14822.30322.30582.34032.35872.38482.39482.41342.43832.45422.45832.47822.49022.52252.53032.55762.57072.5862.59062.59822.60482.61342.61812.62472.62832.63292.63652.63942.63912.63662.63352.62992.62822.62362.62062.61292.60592.59722.59122.57942.56792.55482.54422.52862.52112.50372.49232.47762.46392.44192.42212.39862.38212.35682.3482.31982.30162.27512.26332.23412.21522.17952.15992.12162.10552.07551.98931.96431.90931.88041.84671.81931.76131.74911.72351.71151.69341.68761.68291.66811.65761.6411.62721.6121.59791.58151.57031.55521.49921.4771.44571.42411.3931.36431.33211.31961.30361.29681.28911.28221.27241.26791.26281.26281.26531.27011.27721.28521.29791.30941.32821.34071.36021.37741.40271.41861.43341.45161.48291.50211.51651.5341.54521.55871.57661.59421.6061.6191.62831.63951.64321.64981.65061.651.64591.63971.63461.62721.61731.60711.59841.5851.57431.55621.54351.52381.49611.47461.4511.42581.41021.38091.36551.33251.31231.2831.26091.22621.20771.16951.15251.11421.08751.05071.03641.00010.97280.93570.92210.88530.86530.83040.81080.77830.75320.71870.68350.64590.59920.5650.50880.47450.41220.36040.30890.16170.11850.0507-0.011-0.0759-0.1515-0.1957-0.2371-0.2663-0.2905-0.3247-0.3696-0.4185-0.4667-0.5055-0.5514-0.5802-0.6096-0.6466-0.6756-0.7092-0.7393-0.7844-0.8069-0.8484-0.8803-0.9178-0.948-0.9926-1.0072-1.0506-1.058-1.0947-1.1187-1.1497-1.1709-1.2058-1.2164-1.248-1.2752-1.3067-1.3234-1.3508-1.3617-1.3881-1.3982-1.4208-1.433-1.4508-1.4868-1.5016-1.5044-1.5132-1.5168-1.5205-1.5244-1.5266-1.5267-1.5239-1.5224-1.5174-1.5138-1.5058-1.5001-1.4861-1.4792-1.4229-1.4195-1.3767-1.3666-1.3305-1.3073-1.274-1.2576-1.2295-1.2062-1.1755-1.1537-1.1179-1.0929-1.0568-1.0371-1.001-0.9768-0.9402-0.9268-0.8881-0.8383-0.7997-0.7473-0.7038-0.6403-0.6063-0.5042-0.4675-0.4151-0.383-0.347-0.3276-0.2969-0.2759-0.2491-0.2265-0.201-0.1883-0.1316-0.1206-0.1005-0.074-0.0602-0.01690.00150.03850.0550.09770.11180.15180.18490.2320.27120.30480.35380.39530.45180.49250.5420.58250.64560.69210.75730.80180.91880.95141.00511.03431.06011.08311.10771.12551.14821.16751.191.20191.21741.23151.24751.25891.26971.27661.28461.28891.29291.29461.2951.29341.29161.29091.29161.29541.29961.30631.31531.32641.33461.34831.36061.3781.39311.411.42381.44561.46061.47461.48131.4953];
x_outer=[-2.0865-2.1093-2.1348-2.1528-2.1751-2.1933-2.2195-2.2359-2.2647-2.2829-2.3148-2.3332-2.3587-2.3704-2.3842-2.3971-2.4102-2.422-2.4441-2.4493-2.461-2.4685-2.4766-2.4813-2.4885-2.4908-2.4952-2.4943-2.4963-2.4948-2.4944-2.4934-2.4875-2.484-2.4802-2.4739-2.4641-2.4108-2.3823-2.3618-2.3302-2.3107-2.2782-2.26-2.2529-2.2396-2.1985-2.1752-2.1408-2.0958-2.0759-2.0353-1.9693-1.937-1.8997-1.8788-1.8503-1.8233-1.7778-1.7469-1.7106-1.6838-1.621-1.5917-1.546-1.5005-1.4553-1.4278-1.3983-1.3733-1.3326-1.3044-1.2655-1.2393-1.1937-1.1682-1.1329-1.1077-1.0792-1.0521-1.0084-0.9858-0.9564-0.9396-0.9075-0.8856-0.8575-0.8363-0.8105-0.7909-0.7633-0.7468-0.728-0.7144-0.7005-0.6875-0.6682-0.6567-0.6446-0.6354-0.6268-0.6218-0.6296-0.6132-0.6128-0.5814-0.56-0.5264-0.4728-0.4499-0.4382-0.3977-0.3336-0.3135-0.2747-0.2339-0.1953-0.1526-0.1133-0.0757-0.03810.00190.04120.07290.20320.24330.30040.33950.39850.4480.47940.5070.51160.53580.54810.58120.60680.64730.67140.71050.73120.76420.78290.81540.8380.87350.8910.91920.94640.98050.99711.02181.05211.08181.1111.13821.18251.21241.25481.28331.32581.36491.39851.43021.48261.51931.57091.60791.67351.71411.77681.8081.85071.87621.91571.94651.98942.01992.06742.09842.13832.16642.23942.26072.28942.31282.34742.36882.39672.41562.43582.44992.46992.4832.49572.5072.51932.52532.52992.53432.53742.53682.5352.53172.52412.51612.50512.49392.47972.46592.44822.43122.41542.39712.38552.37162.36592.35752.35422.34952.34662.34842.35022.35532.36072.37382.38582.39942.4082.41842.42732.43982.44962.47232.48332.50582.52642.54482.57362.59372.61812.63372.65812.67392.69132.69992.70972.71382.71822.71982.71842.71492.69732.68382.672.6572.63922.62212.59742.57672.54492.52052.48742.4622.4272.39692.3592.32722.27892.24572.1192.0732.03641.99911.94511.91431.86921.8281.76541.72871.68511.65421.61171.57531.52081.47991.42141.31311.26971.21121.1651.12911.08291.05741.02821.01080.9910.97350.95020.93420.91350.90170.88720.88030.8720.86760.86320.86490.86420.86350.86240.85680.85060.84190.84940.81980.79620.7610.74010.71380.69970.66210.63570.58620.56020.51740.34360.32170.27030.22390.14260.10360.05860.03880.01580.0002-0.0123-0.0222-0.0301-0.0403-0.051-0.0576-0.0638-0.0745-0.0876-0.0977-0.1095-0.1202-0.1317-0.1482-0.1604-0.1832-0.2195-0.2494-0.2704-0.3095-0.3279-0.3645-0.391-0.4234-0.453-0.4941-0.5258-0.5594-0.6028-0.6444-0.6777-0.7224-0.7557-0.7956-0.8335-0.8773-0.9126-0.958-0.991-1.0487-1.0991-1.1474-1.2068-1.257-1.3187-1.3553-1.4109-1.4634-1.5189-1.565-1.6189-1.6668-1.7144-1.7561-1.8073-1.8517-1.8701-1.9161-1.9736-2.0119];
y_outer=[1.25051.26581.2841.29711.31441.32871.35051.36531.39371.41271.45231.47811.51541.53591.55981.58391.61321.63791.69681.71911.74191.76951.80741.83221.87531.90361.94781.973322.02622.04992.07832.12032.14432.17752.21012.24582.40082.45912.49362.53892.56492.60462.62322.63122.64712.68622.70622.73412.76642.77952.80672.84162.85692.87292.88042.89082.89942.91142.9182.92422.92872.93512.9382.93862.93612.9312.92742.92322.91862.912.90232.89062.88192.86352.85172.83412.82092.8042.78852.75882.74132.71832.70362.67522.65312.62272.59912.56552.54012.49972.47152.43832.41192.38472.35552.30722.27152.23022.19192.15432.12442.15362.12862.13012.10112.07932.05192.02612.01392.01111.99911.98181.97611.9651.95031.93711.92041.90531.89011.87531.85881.84111.82611.76181.73961.70831.68671.65271.6241.60541.59291.59251.58571.58271.57571.57051.5661.56281.56281.56221.56691.56831.57641.58331.59481.60251.6151.62771.6451.65451.67051.68891.70711.72791.74711.77511.79271.81591.82951.85041.86791.88351.89651.91521.92641.93861.94521.95061.951.941.93381.92291.91561.90211.89191.87291.85941.83331.81521.78921.76951.70711.68561.65541.63031.5871.55771.51431.48131.44181.41251.36671.33211.2931.25481.19951.16121.12271.08581.03110.99480.94630.90930.85830.82150.77330.73850.69390.66140.62090.58630.55270.51510.48660.45240.43730.40290.38510.33330.31240.16520.14080.07290.0482-0.0168-0.063-0.1072-0.1368-0.1659-0.1876-0.2218-0.2432-0.2921-0.3163-0.3551-0.3901-0.4189-0.4663-0.5032-0.5496-0.5831-0.6401-0.6853-0.746-0.7875-0.8477-0.8853-0.9372-0.9818-1.0314-1.0748-1.1613-1.198-1.2355-1.2664-1.3034-1.3383-1.3808-1.4123-1.4587-1.4902-1.5273-1.5547-1.5868-1.6133-1.6426-1.6652-1.6974-1.7152-1.7724-1.7872-1.7965-1.8053-1.8146-1.8184-1.8239-1.8261-1.8258-1.823-1.8186-1.8137-1.8069-1.7989-1.7838-1.7697-1.7453-1.689-1.6616-1.6188-1.5783-1.5422-1.4893-1.456-1.4158-1.3877-1.3543-1.3236-1.2761-1.2403-1.1861-1.1499-1.0938-1.0578-1.0126-0.976-0.9138-0.8751-0.8441-0.8056-0.7854-0.7419-0.7146-0.6806-0.6927-0.656-0.6365-0.6044-0.5882-0.5688-0.5587-0.5377-0.5221-0.4995-0.4884-0.4757-0.3997-0.3887-0.3611-0.3345-0.2609-0.2176-0.1398-0.1029-0.0478-0.00510.04010.08020.12130.16840.21330.2470.27910.32060.37960.42030.46540.50590.54530.59180.62050.6650.71590.74840.76470.79390.80620.82920.84470.86250.87680.89610.90730.91920.93320.94730.95680.96820.9740.98090.98610.99040.99310.99490.99520.99350.99170.9910.99250.99631.00451.01111.02181.03291.0471.06071.07871.09611.11511.1321.15441.17621.17361.18761.19951.2135];
x_wp=[-2.0028-2.0256-2.0466-2.0646-2.0828-2.101-2.1192-2.1356-2.1565-2.1748-2.1925-2.2109-2.2285-2.2402-2.2519-2.2647-2.2749-2.2868-2.298-2.3031-2.3162-2.3237-2.3293-2.334-2.339-2.3413-2.3453-2.3444-2.3465-2.345-2.3445-2.3435-2.3391-2.3356-2.3329-2.3266-2.3222-2.2689-2.2533-2.2328-2.2103-2.1907-2.1709-2.1527-2.1381-2.1247-2.101-2.0777-2.0535-2.0084-1.9923-1.9517-1.9048-1.8725-1.849-1.828-1.8049-1.778-1.7462-1.7153-1.6856-1.6588-1.6062-1.577-1.5542-1.5087-1.4747-1.4473-1.4256-1.4006-1.3721-1.344-1.3129-1.2867-1.2567-1.2312-1.2021-1.1769-1.1541-1.1271-1.0999-1.0773-1.0551-1.0382-1.014-0.9921-0.969-0.9478-0.9293-0.9096-0.8927-0.8762-0.8615-0.8479-0.8376-0.8247-0.811-0.7994-0.7904-0.7812-0.7748-0.7698-0.755-0.7387-0.7144-0.6829-0.6548-0.6211-0.5434-0.5205-0.4809-0.4404-0.3748-0.3547-0.3257-0.2849-0.2499-0.2072-0.1696-0.132-0.0953-0.0554-0.02330.00830.13070.17080.22790.2670.32340.37290.41750.44510.47110.49530.51720.55030.59010.63070.67110.71030.75270.78570.81920.85160.88420.91970.95180.981.01421.04831.07861.10331.13071.16031.19751.22471.25851.28841.31951.34791.38711.42631.45541.48711.52651.56321.59721.63421.67131.71191.74721.77831.80931.83481.86851.89931.92881.95931.99172.02272.05222.08032.13282.15412.17962.2032.22622.24762.26652.28532.30052.31462.32952.34262.35192.36322.37112.37722.3812.38532.38752.38682.38562.38232.37752.36952.36242.35112.34162.32782.31362.29662.28032.26212.24642.23262.22022.21182.20492.20022.19672.19842.20072.20572.21372.22672.24242.25612.26672.2772.28642.29882.31352.33622.35352.37612.42.41832.44182.46192.4822.49762.51652.53232.54442.5532.56062.56472.56832.56992.56892.56542.55652.5432.53192.51882.50462.48752.47192.45122.42622.40182.37742.3522.32782.29782.2722.24022.20812.17492.07322.02712.00221.96491.92721.89641.86131.821.77711.74031.70891.67791.64381.60741.56961.52871.49061.38231.35831.29971.27131.23531.20211.17671.15571.13821.12141.1041.08721.07121.05611.04431.03451.02761.0211.01661.0131.01471.01421.01351.01121.00560.99590.98720.96610.93640.89740.86220.82930.8030.77290.73530.69790.64840.60320.56040.41090.3890.34470.29820.2540.2150.19090.17110.15670.14110.13330.12350.11650.10630.09620.08960.08140.07080.05790.04780.03560.02490.0096-0.0069-0.0269-0.0497-0.109-0.1389-0.1807-0.2198-0.248-0.2846-0.3187-0.3512-0.3891-0.4301-0.4756-0.5092-0.5547-0.5964-0.6406-0.6853-0.7303-0.7702-0.8186-0.8624-0.9069-0.9523-0.9953-1.0531-1.1013-1.1496-1.1955-1.2457-1.2919-1.3285-1.3798-1.4324-1.4763-1.5224-1.5676-1.6154-1.658-1.6997-1.7413-1.7857-1.8264-1.8724-1.9222-1.9605];
y_wp=[1.3751.39031.40531.41841.43271.44691.4621.47681.49761.51661.53911.56491.58991.61031.63041.65461.67811.70281.73061.75291.78131.80891.83551.86031.88771.91591.94271.96821.99142.01762.04492.07332.09852.12252.14892.18142.1972.3522.38242.41692.44882.47482.49972.51832.53472.55072.57232.59222.61222.64452.65492.68212.70612.72152.73172.73932.74782.75642.76472.77142.77632.78082.78582.78872.78892.78632.78222.77872.77572.77112.76532.75762.74822.73952.72732.71552.7012.68792.67412.65852.63992.62252.60532.59062.56952.54752.52242.49882.47382.44842.42392.39572.36992.34352.3242.29482.26122.22552.19512.15672.12992.12.07152.04652.01971.99081.9631.93561.89371.88151.86731.85531.83761.83191.82391.80921.79741.78071.76631.75111.73661.72011.70571.69071.63051.60831.5771.55541.52281.49421.46881.45631.4481.44121.43591.42891.42151.41691.41281.41281.41371.41851.42271.43081.44061.45211.46541.47791.49391.51121.52861.54461.56111.57941.60541.62461.64581.66341.68051.69411.71351.73111.74481.75781.77171.78291.79091.79751.80061.81.7931.78671.77881.77141.75971.74951.73561.72221.70381.68571.66641.64671.60161.58011.55321.5281.49861.46931.43991.40691.37711.34771.31381.27911.25041.21221.1761.13771.10511.06831.03370.99740.95960.92250.89020.85340.81930.78440.75240.71980.6870.65250.61810.58050.54290.50870.4730.43870.39860.34680.31070.16350.12970.06180.0186-0.0464-0.1073-0.1514-0.187-0.2161-0.239-0.2732-0.3064-0.3553-0.3915-0.4303-0.4708-0.4995-0.5379-0.5749-0.6126-0.6462-0.6897-0.7349-0.7764-0.818-0.864-0.9016-0.9426-0.9872-1.0193-1.0627-1.1097-1.1464-1.1771-1.208-1.2372-1.272-1.2986-1.3302-1.3669-1.3985-1.4254-1.4528-1.4742-1.5007-1.5204-1.543-1.5652-1.583-1.6296-1.6444-1.6505-1.6592-1.6657-1.6694-1.6741-1.6763-1.6763-1.6734-1.6705-1.6656-1.6604-1.6524-1.642-1.6279-1.6123-1.556-1.5406-1.4977-1.4724-1.4363-1.3983-1.365-1.3367-1.3086-1.2803-1.2496-1.2149-1.1791-1.1395-1.1033-1.0655-1.0294-0.9947-0.9581-0.9203-0.8816-0.8412-0.8026-0.7663-0.7228-0.6774-0.6435-0.5985-0.5618-0.5258-0.4937-0.4676-0.4482-0.4278-0.4068-0.3856-0.363-0.3447-0.332-0.2657-0.2547-0.2308-0.2043-0.1606-0.1172-0.0692-0.03220.00360.04630.0760.1160.15310.20020.24230.27590.31640.3580.41570.45640.50370.54420.59540.6420.68890.73340.81740.84990.88490.91410.93320.95620.97620.9941.01251.03181.04861.06061.07531.08941.10221.11361.12191.12871.13531.13971.1431.14471.14511.14341.14171.14091.1421.14581.1521.15871.16851.17971.19081.20451.21971.23711.25411.2711.28911.31091.31711.33111.34041.3544];
Right_curb=[x_inner;y_inner]';
Left_curb=[x_outer;y_outer]';
Driver_pose=[x_wp;y_wp]';%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Find the angles between each waypoint
yaw=zeros(size(Driver_pose,1),1);for i=1:size(Driver_pose,1)-1yaw(i,1)=atan2((Driver_pose(i+1,2)-(Driver_pose(i,2))),(Driver_pose(i+1,1))-(Driver_pose(i,1)));
end
i=size(Driver_pose,1);yaw(i,1)=atan2((Driver_pose(1,2)-(Driver_pose(i,2))),(Driver_pose(1,1))-(Driver_pose(i,1)));
Driver_pose=[Driver_pose, yaw];
Camera_width=3*pi/4;
rand_a=-1;
rand_b=2;
desired_theta=zeros(size(Driver_pose,1),1);
error_theta=zeros(size(Driver_pose,1),1);
rad_vis=1.2;
Frames =[1:size(Driver_pose,1)];%one more round add:1:size(Driver_pose,1)
nFrames =length(Frames);% Preallocate movie structure.mov(1:nFrames)=struct('cdata',[],'colormap',[]);xlabel('x [mts]')ylabel('y [mts]')title('Lane detection simulation in a known map')
grid on
for k = Frames
%for k=10;%tic
%for k=2/arc_length
%L_curve_detected=[];%R_curve_detected=[];
car_distance=mod(k+3.5*(rad_vis)/arc_length,nFrames)+1;
rand_k = car_distance+round(rand_a +(rand_b-rand_a)*rand(1,1));
rand_k = car_distance;%if rand_k >=size(Driver_pose,1)% rand_k =size(Driver_pose,1);%end
Front_vehicle=Driver_pose(round(rand_k),:);%axis tight
axis([-33-33])set(gca,'nextplot','replacechildren');%hold off
%plot(0,0,'w')%hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_p=plot(Right_curb(:,1),Right_curb(:,2),'red.');
hold on
L_p=plot(Left_curb(:,1),Left_curb(:,2),'blue.');
hold on
W_p=plot(Driver_pose(:,1),Driver_pose(:,2),'black:');
hold on
%k=10;
K_p=plot(Driver_pose(k,1),Driver_pose(k,2),'green*');
hold on
F_p=plot(Front_vehicle(1,1),Front_vehicle(1,2),'magenta*');
hold on
C_p=plot(Driver_pose(k,1)+rad_vis*cos(Driver_pose(k,3)+ Camera_width/2),Driver_pose(k,2)+rad_vis*sin(Driver_pose(k,3)+ Camera_width/2),'blue.');
hold on
plot(Driver_pose(k,1)+rad_vis*cos(Driver_pose(k,3)- Camera_width/2),Driver_pose(k,2)+rad_vis*sin(Driver_pose(k,3)- Camera_width/2),'blue.')
hold on
%Plot f1,f2: Left and right border of the vision
X1=Driver_pose(k,1)+rad_vis*cos(-Camera_width/2+Driver_pose(k,3));
X0=Driver_pose(k,1);
Y1=Driver_pose(k,2)+rad_vis*sin(-Camera_width/2+Driver_pose(k,3));
Y0=Driver_pose(k,2);if(X1 > X0)
Bx_min=X0;
Bx_max=X1;
By_min=Y0;
By_max=Y1;else
Bx_min=X1;
Bx_max=X0;
By_min=Y1;
By_max=Y0;
end
PlotX=Bx_min:0.001:Bx_max;
PlotY=By_min:(By_max-By_min)/length(PlotX):By_max-(By_max-By_min)/length(PlotX);
Plotted1=[PlotX;PlotY]';plot(PlotX,PlotY,'blue')
X1_L=Driver_pose(k,1)+rad_vis*cos(Camera_width/2+Driver_pose(k,3));
Y0=Driver_pose(k,2);
Y2=Driver_pose(k,2)+rad_vis*sin(Camera_width/2+Driver_pose(k,3));if(X0 > X1_L)
Bx_min_L=X1_L;
Bx_max_L=X0;
By_min_L=Y2;
By_max_L=Y0;else
Bx_min_L=X0;
Bx_max_L=X1_L;
By_min_L=Y0;
By_max_L=Y2;
end
PlotX=Bx_min_L:0.001:Bx_max_L;
PlotY=By_min_L:(By_max_L-By_min_L)/length(PlotX):By_max_L-(By_max_L-By_min_L)/length(PlotX);
Plotted2=[PlotX;PlotY]';plot(PlotX,PlotY,'blue')
hold on
%Plot f3: Curve section of the cameras vision "the horizon"%for i=[-Camera_width/2:0.01:Camera_width/2 Camera_width/2]% Bx=Driver_pose(k,1)+rad_vis*cos(-i+(Driver_pose(k,3)));% By=Driver_pose(k,2)+rad_vis*sin(-i+(Driver_pose(k,3)));%plot(Bx,By,'blue')% hold on
% end
i=[-Camera_width/2:0.01:Camera_width/2 Camera_width/2];
Bx=Driver_pose(k,1)+rad_vis*cos(-i+(Driver_pose(k,3)));
By=Driver_pose(k,2)+rad_vis*sin(-i+(Driver_pose(k,3)));plot(Bx,By,'blue')
hold on
%Map of the points of the detected features (curbs or lane markers) in the x-y plane
%on the left side
L1_idx=0;
j_all=1:size(Left_curb,1);
L_distance_jk=sqrt((Left_curb(j_all,1)-Driver_pose(k,1)).^2+(Left_curb(j_all,2)-Driver_pose(k,2)).^2);
j_dist=find((L_distance_jk(j_all,1)< rad_vis));%all possible angles
theta_temp=atan2(Left_curb(j_all,2)-Driver_pose(k,2),Left_curb(j_all,1)-Driver_pose(k,1))-(Driver_pose(k,3));
j_minus=find(theta_temp(j_all,1)<-pi );theta_temp(j_minus,1)=theta_temp(j_minus,1)+2*pi;
j_plus=find(theta_temp(j_all,1)> pi );theta_temp(j_plus,1)=theta_temp(j_plus,1)-2*pi;%feasible angles within distance
j_theta=find(abs (theta_temp(j_all,1))<(Camera_width/2));
common_idx=intersect(j_theta,j_dist);
Ld_p=plot(Left_curb(common_idx,1),Left_curb(common_idx,2),'cyan*');
hold on
L_curve_detected=[Left_curb(common_idx,1),Left_curb(common_idx,2),L_distance_jk(common_idx,1)];%Map of the points of the detected features (curbs or lane markers) in the x-y plane
%on the left side
j_all=1:size(Right_curb,1);
R_distance_jk=sqrt((Right_curb(j_all,1)-Driver_pose(k,1)).^2+(Right_curb(j_all,2)-Driver_pose(k,2)).^2);
j_dist=find((R_distance_jk(j_all,1)< rad_vis));%all possible angles
theta_temp=atan2(Right_curb(j_all,2)-Driver_pose(k,2),Right_curb(j_all,1)-Driver_pose(k,1))-(Driver_pose(k,3));
j_minus=find(theta_temp(j_all,1)<-pi );theta_temp(j_minus,1)=theta_temp(j_minus,1)+2*pi;
j_plus=find(theta_temp(j_all,1)> pi );theta_temp(j_plus,1)=theta_temp(j_plus,1)-2*pi;%feasible angles within distance
j_theta=find(abs (theta_temp(j_all,1))<(Camera_width/2));
common_idx=intersect(j_theta,j_dist);
Rd_p=plot(Right_curb(common_idx,1),Right_curb(common_idx,2),'green*');
hold on
R_curve_detected=[Right_curb(common_idx,1),Right_curb(common_idx,2),R_distance_jk(common_idx,1)];%Sort rows depending on distance
L_curve_detected=sortrows(L_curve_detected,3);
R_curve_detected=sortrows(R_curve_detected,3);%Check if one of the curves is empty
min_size_curve_det=5;%Or maybe the half of the other one
%Check the size of the shortest curve
if(size(R_curve_detected,1)<size(L_curve_detected,1))
n_shorter_curve =size(R_curve_detected,1);if((isempty(R_curve_detected))||(size(R_curve_detected,1)< min_size_curve_det))
R_curve_detected = L_curve_detected*((lane_width)^2);
n_shorter_curve =size(R_curve_detected,1);
end
else
n_shorter_curve =size(L_curve_detected,1);if((isempty(L_curve_detected))||(size(L_curve_detected,1)< min_size_curve_det))
L_curve_detected = R_curve_detected/((lane_width)^2);
n_shorter_curve =size(L_curve_detected,1);
end
end
%Sorting the left and right curves depending on distance
if(isempty(n_shorter_curve))
Trajectory_estimator =[0,0];
## ```二、运行结果
## 三、备注
完整代码或者代写添加QQ2449341593
往期回顾>>>>>>[【Matlab 029期】【物理应用1】杨氏双缝干涉实验matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112093463)[【Matlab 055期】【物理应用2】井筒多相流matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112222828)[【Matlab 056期】【物理应用3】Matlab 二维对流扩散温度场](https://blog.csdn.net/TIQCmatlab/article/details/112222950)[【Matlab 057期】【物理应用4】内弹道matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112223038)[【Matlab 058期】【物理应用5】基于两个光纤光栅的新光子微波发生器的matlab仿真分析](https://blog.csdn.net/TIQCmatlab/article/details/112223223)[【Matlab 059期】【物理应用6】matlab计算油气井井底压力](https://blog.csdn.net/TIQCmatlab/article/details/112224643)[【Matlab 061期】【物理应用8】Matlab 带电粒子在放射状电场和均匀磁场下的混沌运动模拟](https://blog.csdn.net/TIQCmatlab/article/details/112257180)[【Matlab 062期】【物理应用9】异构交通流matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112257199)[【Matlab 063期】【物理应用10】模拟斜抛运动matlab源码含GUI界面](https://blog.csdn.net/TIQCmatlab/article/details/112257212)[【Matlab 064期】【物理应用11】 计算晶体结构的x射线衍射图谱matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112257224)[【Matlab 065期】【物理应用12】衍射光栅教学版matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112257236)[【Matlab 066期】【物理应用13】电荷电场线和电势matlab源码](https://blog.csdn.net/TIQCmatlab/article/details/112257254)[【Matlab 067期】【物理应用14】速度场找到漩涡](https://blog.csdn.net/TIQCmatlab/article/details/112257330)[【Matlab 069期】【物理应用16】基于电偶极子的matlab源码GUI](https://blog.csdn.net/TIQCmatlab/article/details/112257402)