227 lines
6.4 KiB
Matlab
227 lines
6.4 KiB
Matlab
% n0= [-1944902.1250000000 4077521.2500000000 4487326.5000000000 ];
|
||
% n1= [-1944873.0000000000 4077511.0000000000 4487348.5000000000 ];
|
||
% n2= [-1944922.0000000000 4077512.0000000000 4487326.5000000000 ];
|
||
% n3= [-1944931.3750000000 4077531.5000000000 4487305.0000000000 ];
|
||
% n4= [-1944882.3750000000 4077530.7500000000 4487326.5000000000 ];
|
||
%
|
||
% n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
|
||
%
|
||
% VN=(cross(n01, n04) + cross(n04, n03) + cross(n03, n02) + cross(n02, n01)) * 0.25;
|
||
% VNorm=VN./norm(VN);
|
||
clear,clc,close all
|
||
|
||
|
||
% 打开二进制文件
|
||
fileID = fopen('E:\\LAMPCAE_SCANE\\outTestEcho\\GF3_Simulation.gpspos.data', 'rb'); % 假设二进制文件名为 'data.bin'
|
||
|
||
% 定义每个数据字段的数据类型
|
||
% 假设每个数据是双精度浮动数（8字节）
|
||
dataType = 'double';
|
||
|
||
% 读取数据：根据你的数据格式，数据长度为 16 * 8 字节（每个元素8字节）
|
||
% 例如：每16个数据项为一组（对应prf_id）
|
||
numFields = 19; % 每组数据的字段数
|
||
PRFnumRecords = 15382; % 假设有100个记录，你可以根据实际情况修改
|
||
|
||
% 读取所有记录的数据
|
||
rawData = fread(fileID, numFields * PRFnumRecords, dataType);
|
||
|
||
% 关闭文件
|
||
fclose(fileID);
|
||
|
||
% 解析数据为结构体数组
|
||
data = struct();
|
||
AntDirecX=zeros(PRFnumRecords,1);
|
||
AntDirecY=zeros(PRFnumRecords,1);
|
||
AntDirecZ=zeros(PRFnumRecords,1);
|
||
Px=zeros(PRFnumRecords,1);
|
||
Py=zeros(PRFnumRecords,1);
|
||
Pz=zeros(PRFnumRecords,1);
|
||
|
||
Vx=zeros(PRFnumRecords,1);
|
||
Vy=zeros(PRFnumRecords,1);
|
||
Vz=zeros(PRFnumRecords,1);
|
||
|
||
% 假设你的数据在 rawData 中按顺序排列
|
||
% 例如，解析 prf_id = 0, 1, 2...的相关数据
|
||
for prf_id = 0:PRFnumRecords-1
|
||
idx = prf_id * numFields + 1;
|
||
data(prf_id+1).prf_time = rawData(idx); % prf_time + imageStarttime
|
||
Px(prf_id+1) = rawData(idx+1); % sateOirbtNode.Px
|
||
Py(prf_id+1) = rawData(idx+2); % sateOirbtNode.Py
|
||
Pz(prf_id+1) = rawData(idx+3); % sateOirbtNode.Pz
|
||
Vx(prf_id+1) = rawData(idx+4); % sateOirbtNode.Vx
|
||
Vy(prf_id+1) = rawData(idx+5); % sateOirbtNode.Vy
|
||
Vz(prf_id+1) = rawData(idx+6); % sateOirbtNode.Vz
|
||
AntDirecX(prf_id+1) = rawData(idx+7); % sateOirbtNode.AntDirecX
|
||
AntDirecY(prf_id+1) = rawData(idx+8); % sateOirbtNode.AntDirecY
|
||
AntDirecZ(prf_id+1) = rawData(idx+9); % sateOirbtNode.AntDirecZ
|
||
data(prf_id+1).AVx = rawData(idx+10); % sateOirbtNode.AVx
|
||
data(prf_id+1).AVy = rawData(idx+11); % sateOirbtNode.AVy
|
||
data(prf_id+1).AVz = rawData(idx+12); % sateOirbtNode.AVz
|
||
data(prf_id+1).lon = rawData(idx+13); % outP.lon
|
||
data(prf_id+1).lat = rawData(idx+14); % outP.lat
|
||
data(prf_id+1).ati = rawData(idx+15); % outP.ati
|
||
end
|
||
|
||
% 查看解析后的数据
|
||
disp(data);
|
||
|
||
|
||
|
||
fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageX.dat', 'rb'); % 假设二进制文件名为 'data.bin'
|
||
dataType = 'float';
|
||
numFields = 5400; % 每组数据的字段数
|
||
numRecords = 5400; % 假设有100个记录，你可以根据实际情况修改
|
||
demX = fread(fileID, numFields * numRecords, dataType);
|
||
demX=reshape(demX,[numRecords,numRecords]);
|
||
fclose(fileID);
|
||
|
||
|
||
fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageY.dat', 'rb'); % 假设二进制文件名为 'data.bin'
|
||
dataType = 'float';
|
||
numFields = 5400; % 每组数据的字段数
|
||
numRecords = 5400; % 假设有100个记录，你可以根据实际情况修改
|
||
demY = fread(fileID, numFields * numRecords, dataType);
|
||
demY=reshape(demY,[numRecords,numRecords]);
|
||
fclose(fileID);
|
||
|
||
fileID = fopen('D:\\Programme\\vs2022\\RasterMergeTest\\TestData\\outData\\tmp\\ImageZ.dat', 'rb'); % 假设二进制文件名为 'data.bin'
|
||
dataType = 'float';
|
||
numFields = 5400; % 每组数据的字段数
|
||
numRecords = 5400; % 假设有100个记录，你可以根据实际情况修改
|
||
demZ = fread(fileID, numFields * numRecords, dataType);
|
||
demZ=reshape(demZ,[numRecords,numRecords]);
|
||
fclose(fileID);
|
||
|
||
|
||
%% 绘制成像平面
|
||
Rn=884053;
|
||
Rf=942069;
|
||
|
||
|
||
data_ant_norm=(AntDirecX.^2+AntDirecY.^2+AntDirecZ.^2).^0.5;
|
||
|
||
AntDirecX=AntDirecX./data_ant_norm;
|
||
AntDirecY=AntDirecY./data_ant_norm;
|
||
AntDirecZ=AntDirecZ./data_ant_norm;
|
||
|
||
Rlist=linspace(Rn,Rf,300);
|
||
ImageX=zeros(PRFnumRecords,300);
|
||
ImageY=zeros(PRFnumRecords,300);
|
||
ImageZ=zeros(PRFnumRecords,300);
|
||
|
||
for i=1:PRFnumRecords
|
||
ImageX(i,:)=Px(i,:) + Rlist.*AntDirecX(i,:);
|
||
ImageY(i,:)=Py(i,:) + Rlist.*AntDirecY(i,:);
|
||
ImageZ(i,:)=Pz(i,:) + Rlist.*AntDirecZ(i,:);
|
||
end
|
||
B=[AntDirecX(1,1),AntDirecY(1,1),AntDirecZ(1,1)];
|
||
A=[Px(1,1)-Px(2,1),Py(1,1)-Py(2,1),Pz(1,1)-Pz(2,1)] ;
|
||
V=[Vx(1,1),Vy(1,1),Vz(1,1)];
|
||
% 计算点积
|
||
dotProduct = dot(A,B);
|
||
|
||
% 计算模长
|
||
normA = norm(A);
|
||
normB = norm(B);
|
||
|
||
% 计算夹角的余弦值
|
||
cosTheta = acos(dotProduct / (normA * normB));
|
||
cosTheta*180/pi
|
||
|
||
tp=[-2023401.250000,4104205.500000,4428297.000000];
|
||
TP=Px.*0;
|
||
for i=1:PRFnumRecords
|
||
TP(i,:)=sqrt((Px(i,:) -tp(1)).^2+(Py(i,:) -tp(2)).^2+(Pz(i,:) -tp(3)).^2);
|
||
|
||
end
|
||
|
||
figure,
|
||
plot(TP )
|
||
|
||
|
||
|
||
PR=(Px.^2+Py.^2+Pz.^2).^0.5;
|
||
|
||
|
||
|
||
ImageR=(ImageX.^2+ImageY.^2+ImageZ.^2).^0.5;
|
||
|
||
|
||
figure,
|
||
plot(ImageR(:,1)),hold on
|
||
plot(ImageR(:,end)),hold on
|
||
plot(PR),hold on
|
||
legend(["Rn","Rf","Rs"])
|
||
|
||
a=[Px-mean(demX,"all"),Py-mean(demY,"all"),Pz-mean(demZ,"all")];
|
||
a=(a(:,1).^2+a(:,2).^2+a(:,3).^2).^0.5;
|
||
|
||
|
||
|
||
figure,
|
||
surfl(ImageX,ImageY,ImageZ),shading flat,hold on
|
||
scatter3(mean(demX,"all"),mean(demY,"all"),mean(demZ,"all"),'red','filled'),shading flat,hold on
|
||
plot3(Px,Py,Pz,'red')
|
||
xlabel("X")
|
||
ylabel("Y")
|
||
zlabel("Z")
|
||
view([-90.6000010944003 90]);
|
||
|
||
|
||
dl=sqrt((Px(1)-Px(end)).^2+(Py(1)-Py(end)).^2+(Pz(1)-Pz(end)).^2);
|
||
|
||
R=7;
|
||
ImageR=((Px(10000,1)-R-Px).^2+(Py(10000,1)-R-Py).^2+(Pz(10000,1)-R-Pz).^2).^0.5;
|
||
figure,
|
||
plot(ImageR)
|
||
|
||
|
||
P=zeros(100*100,3);
|
||
pp=1;
|
||
for i=1:100
|
||
for j=1:100
|
||
[latitude,longitude,height]=XYZ2LLA(ImageX(i,j),ImageY(i,j),ImageZ(i,j));
|
||
P(pp,:)=[latitude,longitude,height];
|
||
pp=pp+1;
|
||
end
|
||
end
|
||
|
||
|
||
|
||
|
||
|
||
% %% 计算
|
||
%
|
||
%
|
||
% Tx=-2001212.875000;
|
||
% Ty=4082207.000000;
|
||
% Tz=4471714.500000;
|
||
% R=((demX-Px(1)).^2+(demY-Py(1)).^2+(demZ-Pz(1)).^2).^0.5;
|
||
% figure,
|
||
% plot(R)
|
||
%
|
||
% imgX=ImageX(3000,150);
|
||
% imgY=ImageY(3000,150);
|
||
% imgZ=ImageZ(3000,150);
|
||
% R1= sqrt((ImageX(1,1)-Px(:,1)).^2+(ImageY(1,1)-Py(:,1)).^2+(ImageZ(1,1)-Pz(:,1)).^2);
|
||
% figure,
|
||
% plot(R1)
|
||
%
|
||
%
|
||
%
|
||
% Timgx=abs(ImageX-Tx);
|
||
% Timgy=abs(ImageY-Ty);
|
||
% Timgz=abs(ImageZ-Tz);
|
||
%
|
||
% [latitude,longitude,height]=XYZ2LLA(ImageX(3000,150),ImageY(3000,150),ImageZ(3000,150))
|
||
%
|
||
%
|
||
%
|
||
|
||
|
||
|
||
|
||
|