基于matlab实现卡尔曼滤波器完成多传感器数据融合 对多个机器人的不同传感器数据进行融合估计足球精确位置.rar

% Simulations of Distributed Sensor Fusion for Object Position Estimation by Multi-Robot % Systems % Wu Botao % 2011-10-29 clear; theta1 = 30/180*pi; theta2 = 45/180*pi; theta3 = 60/180*pi; R1 = [cos(-theta1) sin(-theta1);-sin(-theta1) cos(-theta1)]; % transform matirx R2 = [cos(-theta2) sin(-theta2);-sin(-theta2) cos(-theta2)]; % transform matirx R3 = [cos(-theta3) sin(-theta3);-sin(-theta3) cos(-theta3)]; % transform matirx C10 = [25 0;0 9]; % covariance matrix of readings from robot 1 C20 = [9 0;0 1]; % covariance matrix of robot 2 C30 = [16 0;0 1] % covariance matrix of robot 3 X1 = [12.34;9.02]; % distribution mean of robot 1 X2 = [9.90;11.69]; % distribution mean of robot 2 X3 = [10.90;9.69]; % distribution mean of robot 3 X = [0;0]; C1 = R1'*C10*R1; % rotation processing C2 = R2'*C20*R2; % rotation processing C3 = R3'*C30*R3; % rotation processing % merged Robot 1 and Robot 2 C4 = C1-C1/[C1+C2]*C1; % Combine two covariance matrices into a single convariance matrix X4 = X1 + C1/[C1+C2]*(X2-X1); % Calculation of mean of resulting merged distribution theta4 = 1/2*atan(2*C4(1,1)/(C4(1,2)-C4(2,2))); % Calculation of angle of the resulting principal axis R4 = [cos(theta4) sin(theta4);-sin(theta4) cos(theta4)]; C0 = R4'*C4*R4; % rotating the covariance % merge former result with Robot 3 C = C3-C3/[C3+C4]*C3; X = X3+C3/[C3+C4]*(X4-X3); theta = 1/2*atan(2*C(1,1)/(C(1,2)-C(2,2))); R = [cos(theta) sin(theta);-sin(theta) cos(theta)]; C = R'*C*R; TempP=400; [x1,x2]=meshgrid(linspace(0,30,TempP)', linspace(0,30,TempP)'); x=[x1(:) x2(:)]; % Observation Robot 1 figure(1); p=mvnpdf(x, X1', C1); mesh(x1,x2,reshape(p,TempP,TempP)); title('Observation Robot 1'); % Observation Robot 2 figure(2); p=mvnpdf(x, X2', C2); mesh(x1,x2,reshape(p,TempP,TempP)); title('Observation Robot 2'); % Observation Robot 3 figure(3); p=mvnpdf(x, X3', C3); mesh(x1,x2,reshape(p,TempP,TempP)); title('Observation Robot 3'); % Merged Observation figure(4); p=mvnpdf(x, X', C); mesh(x1,x2,reshape(p,TempP,TempP)); title('Merged Observation');