simulation.m

扩展卡尔曼滤波算法是滤波领域较为重要的方法之一。本滤波算法是典型的卡尔曼滤波应用问题。

%localization using a kalman filter approach
clear all;
%simulation parameter
global diameter_robot measure_var odomtrie_var ...
	   visu_time hold_rate width X_0 init_kalmann P_0
diameter_robot      = 1;            %diameter of the robot for simulation demonstration
X_0                 = [-8 -8 pi/2]'; %init position                   
width               = 4;            %width of the corridor  
visu_time           = 0.01;         %updating time for simulation
hold_rate           = 10;           %number of iterations between to fixed plots
init_kalmann        = 1;            %for initialization of kalman filter
P_0                 = eye(3,3)*0.1; %no doubt about initposition

disp('startposition');
visualize(0,X_0);

disp('start simulation');

%assignement1)
robot_state       = X_0;
robot_state_noise = X_0;
odomtrie_var      = [0.001;0.0001]    %odometrie covariance
measure_var       = [1e-5;1e-5;1e-5]; %measurement covariance

% translation 
control.delta=0.1;
control.omega=0.0;
for i=1:160
    delta                             = [control.delta control.omega];
    robot_state                       = vehicle(delta,robot_state);
    robot_state_noise                 = vehicle_noise(delta,robot_state_noise);
    xywpose_noisymeasure              = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
    information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
    visualize(1,robot_state_noise,xywpose_noisymeasure,information); 
    visualize(2,robot_state);
    %visualize(3,robot_state_estimate);
end 


% % rotation
% control.delta=0.0;
% control.omega=-pi/100;
% for i=161:210
%     delta                        = [control.delta control.omega];
%     robot_state                  = vehicle(delta,robot_state);
%     robot_state_noise            =
%     vehicle_noise(delta,robot_state_noise);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  = ekf_filter(delta,xywpose_noisymeasure);
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information);  
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end
% 
% % translation 
% control.delta=0.1;
% control.omega=0.0;
% for i=211:370
%     delta                        = [control.delta control.omega];
%     robot_state                  = vehicle(delta,robot_state);
%     robot_state_noise            = vehicle_noise(delta,robot_state_noise);
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information); 
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end 
% 
% % rotation
% control.delta=0.0;
% control.omega=-pi/100;
% for i=371:420
%     delta        = [control.delta control.omega];
%     robot_state  = vehicle(delta,robot_state);
%     robot_state_noise            = vehicle_noise(delta,robot_state_noise);
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information); 
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end
% 
% % translation 
% control.delta=0.1;
% control.omega=0.0;
% for i=421:580
%     delta        = [control.delta control.omega];
%     robot_state  = vehicle(delta,robot_state);
%     robot_state_noise            = vehicle_noise(delta,robot_state_noise);
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information);  
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end 
% 
% % rotation
% control.delta=0.0;
% control.omega=-pi/100;
% for i=581:630
%     delta        = [control.delta control.omega];
%     robot_state  = vehicle(delta,robot_state);
%     robot_state_noise            = vehicle_noise(delta,robot_state_noise);
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information); 
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end
% 
% % translation 
% control.delta=0.1;
% control.omega=0.0;
% for i=631:790
%     delta        = [control.delta control.omega];
%     robot_state  = vehicle(delta,robot_state);
%     robot_state_noise            = vehicle_noise(delta,robot_state_noise);
%     xywpose_noisymeasure         = measurement_noise(robot_state_noise);
%     [robot_state_estimate,cov_noise]  =
%     ekf_filter(delta,xywpose_noisymeasure);
%     information                  = sprintf('delta w: %0.5g \ndelta v: %0.5g',delta(2),delta(1));
%     visualize(1,robot_state_noise,xywpose_noisymeasure,information);  
%     visualize(2,robot_state);
%     visualize(3,robot_state_estimate);
% end