matlab根据.mat文件绘制绘制多个子图、多条曲线

%------------------------两个虚线间变量需要改----------------------------------------------------%
%加载mat
veh = load('0921.mat');     %运行模型后生成的a_***存成的mat


%fusn source = 1(radar),2(cmr),3(fusn)
%设置时间序列中，绘制曲线的开始 结束时间。
start_time = 0;
end_time = 6.5;

%seeker萨和嗯要观测的目标的视觉ID
cmr_id_seeker = 9394;

cmr_id = mod(cmr_id_seeker, 200) + 1;  %veh，目标是车将这行打开。

% cmr_id = mod(cmr_id_seeker, 55) +201; %bicy  person，目标是人和而轮车开启这行。

text_frameid = "\leftarrow ";
%可以设置在某一时刻，在曲线上画一个箭头
text_framrid_time = 0;
%----------------------------------------------------------------------------%


fusn_id =5;

% text_frameID = num2str(text_frameid);


yawrate = veh.a_veh_yawrate.Data;
vlgt = veh.a_veh_vlgt.Data;
algt = veh.a_veh_algt.Data;
frameid = veh.a_fusn_frameid.Data;
% disp(frameid);
fusn_source =0;


time = veh.a_RadarCamObj.Objects(fusn_id).Estimate.latPosition.Time;

% id_cmr = veh.a_RadarCamObj.Objects.;
% disp(id_cmr);
len = 0;
for i =1:length(time)
    data_time = veh.a_RadarCamObj.Objects(fusn_id).Estimate.latPosition.Time(i);
    if data_time>start_time && data_time < end_time
        len = len + 1;

    end
end

drawID = zeros(len,1);
drawtime = zeros(len,1);

xpos_cmr = zeros(len,1);
ypos_cmr = zeros(len,1);
xvel_cmr = zeros(len,1);
yvel_cmr = zeros(len,1);

xpos_radar = zeros(len,1);
ypos_radar = zeros(len,1);
xvel_radar = zeros(len,1);
yvel_radar = zeros(len,1);
xacc_radar = zeros(len,1);
yacc_radar = zeros(len,1);

xpos_fusn = zeros(len,1);
ypos_fusn = zeros(len,1);
xvel_fusn = zeros(len,1);
yvel_fusn = zeros(len,1);
xacc_fusn = zeros(len,1);
yacc_fusn = zeros(len,1);

text_xpos_fusn = 0;
text_ypos_fusn = 0;
text_xvel_fusn =0;
text_yvel_fusn =0;
text_xacc_fusn =0;
text_yacc_fusn =0;
fusn_xpos = 0;
fusn_ypos = 0;  

fusn_xvel = 0;  
fusn_yvel = 0;

fusn_xacc = 0;  
fusn_yacc= 0;
count = 0;

for i =1:length(time)
    data_time = veh.a_RadarCamObj.Objects(fusn_id).Estimate.latPosition.Time(i);
    % frame_id =  veh.a_fusn_frameid.Data(i);

    % fusn_xpos = veh.a_RadarCamObj.Objects(fusn_id).Estimate.longPosition.Data(i);
    % fusn_ypos = veh.a_RadarCamObj.Objects(fusn_id).Estimate.latPosition.Data(i);  

    % fusn_xvel = veh.a_RadarCamObj.Objects(fusn_id).Estimate.longVelocity.Data(i);  
    % fusn_yvel = veh.a_RadarCamObj.Objects(fusn_id).Estimate.latVelocity.Data(i);

    % fusn_xacc = veh.a_RadarCamObj.Objects(fusn_id).Estimate.longAcceleration.Data(i);  
    % fusn_yacc= veh.a_RadarCamObj.Objects(fusn_id).Estimate.latAcceleration.Data(i);


    

    if data_time>start_time && data_time < end_time
        count = count +1;


        for j = 1:32

            fusn_visnID = veh.a_RadarCamObj.Objects(j).Properties.visionId.Data(i);
            % disp(fusn_visnID);
            if fusn_visnID == cmr_id
                
                fusn_sou = veh.a_RadarCamObj.Objects(j).Properties.fusionSource.Data(i);
                % disp(fusn_sou)

                if fusn_sou == 2
                    fusn_source =2;
                    fusn_xpos = veh.a_RadarCamObj.Objects(j).Estimate.longPosition.Data(i);
                    fusn_ypos = veh.a_RadarCamObj.Objects(j).Estimate.latPosition.Data(i);  

                    fusn_xvel = veh.a_RadarCamObj.Objects(j).Estimate.longVelocity.Data(i);  
                    fusn_yvel = veh.a_RadarCamObj.Objects(j).Estimate.latVelocity.Data(i);

                    fusn_xacc = veh.a_RadarCamObj.Objects(j).Estimate.longAcceleration.Data(i);  
                    fusn_yacc= veh.a_RadarCamObj.Objects(j).Estimate.latAcceleration.Data(i);

                elseif fusn_sou == 3
                    radar_id = veh.a_RadarCamObj.Objects(j).Information.height.Data(i);
                    fusn_source = 3;
                    fusn_xpos = veh.a_RadarCamObj.Objects(j).Estimate.longPosition.Data(i);
                    fusn_ypos = veh.a_RadarCamObj.Objects(j).Estimate.latPosition.Data(i);  

                    fusn_xvel = veh.a_RadarCamObj.Objects(j).Estimate.longVelocity.Data(i);  
                    fusn_yvel = veh.a_RadarCamObj.Objects(j).Estimate.latVelocity.Data(i);

                    fusn_xacc = veh.a_RadarCamObj.Objects(j).Estimate.longAcceleration.Data(i);  
                    fusn_yacc= veh.a_RadarCamObj.Objects(j).Estimate.latAcceleration.Data(i);

                else
                    fusn_source = fusn_sou;
                    radar_id = veh.a_RadarCamObj.Objects(j).Information.height.Data(i);
                    fusn_xpos = veh.a_RadarCamObj.Objects(j).Estimate.longPosition.Data(i);
                    fusn_ypos = veh.a_RadarCamObj.Objects(j).Estimate.latPosition.Data(i);  

                    fusn_xvel = veh.a_RadarCamObj.Objects(j).Estimate.longVelocity.Data(i);  
                    fusn_yvel = veh.a_RadarCamObj.Objects(j).Estimate.latVelocity.Data(i);

                    fusn_xacc = veh.a_RadarCamObj.Objects(j).Estimate.longAcceleration.Data(i);  
                    fusn_yacc= veh.a_RadarCamObj.Objects(j).Estimate.latAcceleration.Data(i);
                end

            end
            

        end

        drawtime(count,1) = data_time;
        % drawID(count,1) = frame_id;
        xpos_fusn(count,1) = fusn_xpos;
        ypos_fusn(count,1) = fusn_ypos;
        xvel_fusn(count,1) = fusn_xvel;
        yvel_fusn(count,1) = fusn_yvel;
        xacc_fusn(count,1) = fusn_xacc;
        yacc_fusn(count,1) = fusn_yacc;

        


        for j = 1:21

            id_cmr = veh.a_CmrObjList.objectsVec.objects(j).id.Data(i);

            if id_cmr==cmr_id
                % disp(veh.a_CmrObjList.objectsVec.objects(j).id);
                xpos_cmr(count,1) =  veh.a_CmrObjList.objectsVec.objects(j).xPos.Data(i);
                ypos_cmr(count,1) =  veh.a_CmrObjList.objectsVec.objects(j).yPos.Data(i);
                xvel_cmr(count,1) =  veh.a_CmrObjList.objectsVec.objects(j).xRelVel.Data(i);
                yvel_cmr(count,1) =  veh.a_CmrObjList.objectsVec.objects(j).yRelVel.Data(i);

            end
            % visionid = 1;
        end
        % disp(data_time);
        % disp(radar_id);
        if (fusn_source == 3 || fusn_source == 1) && radar_id > 0
            for j = 1:40

                id_radar = veh.a_RdrObjList.objectsVec.objects(j).id.Data(i);
                
                
                if id_radar == radar_id
                    % disp(veh.a_CmrObjList.objectsVec.objects(j).id);

                    xpos_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).xPos.Data(i);
                    ypos_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).yPos.Data(i);
                    f32YawRateSq = yawrate(i)*yawrate(i);
                    f32YawRateTwo = 2.0 * yawrate(i);

                    radar_xVel = veh.a_RdrObjList.objectsVec.objects(j).xVel.Data(i);
                    radar_yVel = veh.a_RdrObjList.objectsVec.objects(j).yVel.Data(i);
                    radar_xAcc = veh.a_RdrObjList.objectsVec.objects(j).xAcc.Data(i);
                    radar_yAcc = veh.a_RdrObjList.objectsVec.objects(j).yAcc.Data(i);
                    xacc_radar(count,1) = (((((-f32YawRateSq) * xpos_radar(count,1)) - (f32YawRateTwo * radar_yVel)) + radar_xAcc) + algt(i));
                    %[TRACKABLE_ACCY] = ((((-f32YawRateSq) * f32PosY) +  (f32YawRateTwo * f32VelX)) + f32AccY) + ((stEgoMotion->f32YawRate) * (stEgoMotion->f32Speed));
                    yacc_radar(count,1)= ((((-f32YawRateSq) * ypos_radar(count,1)) +  (f32YawRateTwo * radar_xAcc)) + radar_yAcc) + (yawrate(i) * vlgt(i));
                
                    xvel_radar(count,1) = ((-yawrate(i)* ypos_radar(count,1)) + radar_xVel) + vlgt(i);     %[TRACKABLE_VELX] = ((-stEgoMotion->f32YawRate * f32PosY) + f32VelX) + (stEgoMotion->f32Speed);
                    yvel_radar(count,1) = ((yawrate(i) * xpos_radar(count,1)) + radar_yVel);   %[TRACKABLE_VELY] = (((stEgoMotion->f32YawRate) * f32PosX) + f32VelY);

                    % xvel_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).xVel.Data(i);
                    % yvel_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).yVel.Data(i);

                    % xacc_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).xAcc.Data(i);
                    % yacc_radar(count,1) = veh.a_RdrObjList.objectsVec.objects(j).yAcc.Data(i);

                end
                continue;
                % visionid = 1;

            end
        else
            xpos_radar(count,1) = 0;
            ypos_radar(count,1) = 0;
            xacc_radar(count,1) = 0;
            yacc_radar(count,1)= 0;
        
            xvel_radar(count,1) = 0;
            yvel_radar(count,1) = 0;
        end
    end

    if data_time == text_framrid_time
        % disp(data_time)
        text_x = text_framrid_time;
        text_xpos_fusn = fusn_xpos;
        % disp(fusn_xpos);
        text_ypos_fusn = fusn_ypos;
        text_xvel_fusn =fusn_xvel;
        text_yvel_fusn =fusn_yvel;
        text_xacc_fusn =fusn_xacc;
        text_yacc_fusn =fusn_yacc;
    end

end


subplot(3,2,1);
plot(drawtime,xpos_fusn,'r-', drawtime, xpos_radar,'b-',drawtime, xpos_cmr,'g-')
text(text_xpos_fusn,text_ypos_fusn,text_frameid);
legend('fusn','radar','cmr');
text(text_x, text_xpos_fusn, text_frameid,'FontSize',20);
title('pos_x')

subplot(3,2,2);
plot(drawtime,ypos_fusn,'r-', drawtime, ypos_radar,'b-',drawtime, ypos_cmr,'g-')
legend('fusn','radar','cmr');
text(text_x, text_ypos_fusn, text_frameid,'FontSize',20);
title('pos_y')

subplot(3,2,3);
plot(drawtime,xvel_fusn,'r-', drawtime, xvel_radar,'b-',drawtime, xvel_cmr,'g-')
legend('fusn','radar','cmr');
text(text_x, text_xvel_fusn, text_frameid,'FontSize',20);
title('vel_x')

subplot(3,2,4);
plot(drawtime,yvel_fusn,'r-', drawtime, yvel_radar,'b-',drawtime, yvel_cmr,'g-')
legend('fusn','radar','cmr');
text(text_x, text_yvel_fusn, text_frameid,'FontSize',20);
title('vel_y')

subplot(3,2,5);
plot(drawtime,xacc_fusn,'r-', drawtime, xacc_radar,'b-')
legend('fusn','radar');
text(text_x, text_xacc_fusn, text_frameid,'FontSize',20);
title('acc_x')

subplot(3,2,6);
plot(drawtime,yacc_fusn,'r-', drawtime, yacc_radar,'b-')
legend('fusn','radar');
text(text_x, text_yacc_fusn, text_frameid,'FontSize',20);
title('acc_y')