用kalman进行多传感器信息融合
在这里插入代码片clc;
clear all;
close all;
% longitude(1)=0;
% latitude(1)=0;
y(1) = 521.66;
j(1) = 522.33;
f(1) = 521.94;
% y(1)=59.1023;
% j(1)=59.1023;
% f(1)=57.5564;
fid=fopen('1111.txt'); %打开数据总文件
B=textscan(fid,'%f %f');%把每一列的数据读入到读入到单元数组B中
C=[B{1} B{2}]; %从单元数组B中提取每列数据赋值给矩阵C
n=size(C,1); %确定读入数据的坐标数目
longitude=C(:,1);latitude=C(:,2); %赋值
fclose(fid);
fod=fopen('2222.txt'); %打开数据总文件
A=textscan(fod,'%f %f');%把每一列的数据读入到读入到单元数组B中
G=[A{1} A{2}]; %从单元数组B中提取每列数据赋值给矩阵G
num=size(G,1); %确定读入数据的坐标数目
longitude2=G(:,1);latitude2=G(:,2); %赋值
fclose(fod);
fud=fopen('3333.txt'); %打开数据总文件
S=textscan(fud,'%f %f');%把每一列的数据读入到读入到单元数组S中
F=[S{1} S{2}]; %从单元数组S中提取每列数据赋值给矩阵F
num1=size(F,1); %确定读入数据的坐标数目
%=数据的读取======================================================
w1=var(longitude_fil-y);
w2=var(longitude2_fil-j);
w3=var(longitude3_fil-f);
disp(‘w1=’);disp(w1);
disp(‘w2=’);disp(w2);
disp(‘w3=’);disp(w3);
result=(w3/(w1+w2+w3))*longitude_fil+(w1/(w1+w2+w3))*longitude2_fil+(w2/(w1+w2+w3))*longitude3_fil;
% result=(w2/(w1+w2))*longitude_fil+(w1/(w1+w2))*longitude2_fil;
% w3=var(result);
% disp(‘w3=’);disp(w3);
% disp(‘result=’);disp(result);
for i=1:num;
dlmwrite(’.\result.txt’,result,‘delimiter’,’%10.000f\t’);
%MSE3=(sum(result(i)-longitude(i)))/num;
end
figure(1);
hold on;
plot(1:n,Err1,’-or’);
plot(1:n,Err2,’-+k’);
grid on;
hold off;
legend(‘滤波前误差’,‘滤波后误差’);
xlabel(‘采样次数’,‘color’,‘b’);
ylabel(‘幅值’,‘color’,‘b’);
title(‘坐标图线’,‘color’,‘m’);
figure(2);
hold on;
plot(1:num,Err3,’-or’);
plot(1:num,Err4,’-+k’);
grid on;
hold off;
legend(‘滤波前误差’,‘滤波后误差’);
xlabel(‘采样次数’,‘color’,‘b’);
ylabel(‘幅值’,‘color’,‘b’);
title(‘坐标图线’,‘color’,‘m’);
figure(3);
hold on;
plot(1:num1,Err5,’-or’);
plot(1:num1,Err6,’-+k’);
grid on;
hold off;
legend(‘滤波前误差’,‘滤波后误差’);
xlabel(‘采样次数’,‘color’,‘b’);
ylabel(‘幅值’,‘color’,‘b’);
title(‘坐标图线’,‘color’,‘m’);
figure(7);
hold on;
plot(1:num,longitude_fil,’-+k’);
plot(1:num,longitude2_fil,’-*b’);
plot(1:num,longitude3_fil,’-xy’);
plot(1:num,result,’-og’);
grid on;
%plot(1:n,result,’-*r’);
hold off;
legend(‘滤波后1’,‘滤波后2’,‘滤波后3’,‘融合后’);
xlabel(‘采样次数’,‘color’,‘b’);
ylabel(‘幅值’,‘color’,‘b’);
title(‘坐标图线’,‘color’,‘m’);
!最后做ji在这里插入图片描述加权均值融合处理
这里是引用https://blog.csdn.net/u010442908/article/details/81416960?ops_request_misc=&request_id=&biz_id=102&utm_term=kalman%E4%BB%BF%E7%9C%9F&utm_medium=distribute.pc_search_result.none-task-blog-2allsobaiduweb~default-0-81416960
z