无源时差定位代码

clc
clear all
close all

radar0=[0,0,0];   %主 被动雷达
radar1=[100,0,0]; %副
radar2=[0,100,0]; %副

syms x y z

radar3=[0,0,100]; %副
target=[100,300,400];
deta_t01=1; %时间差
deta_t02=1; %时间差
deta_t03=1; %时间差
v=100;
deta_R01=9.9020; %距离差
deta_R02=51.6444; %距离差
deta_R03=74.0121; %距离差

R0=sqrt((target(1)-radar0(1))^2+(target(2)-radar0(2))^2+(target(3)-radar0(3))^2);
% R2=sqrt((target(1)-radar2(1))^2+(target(2)-radar2(2))^2);

A=[radar1(1)-radar0(1) radar1(2)-radar0(2) radar1(3)-radar0(3)
   radar2(1)-radar0(1) radar2(2)-radar0(2) radar2(3)-radar0(3)
   radar3(1)-radar0(1) radar3(2)-radar0(2) radar3(3)-radar0(3)
];
r=[radar1(1)^2+radar1(2)^2+radar1(3)^2
   radar2(1)^2+radar2(2)^2+radar2(3)^2
   radar3(1)^2+radar3(2)^2+radar3(3)^2
];
D=[ (r(1)-deta_R01^2)/2
    (r(2)-deta_R02^2)/2
    (r(3)-deta_R03^2)/2

];
F=[D(1)+R0*deta_R01
   D(2)+R0*deta_R02
    D(3)+R0*deta_R03
];

inv(A)*F

figure;
plot3(radar1(1),radar1(2),radar1(3),'*');
hold on
plot3(radar2(1),radar2(2),radar1(3),'*');

plot3(radar3(1),radar3(2),radar3(3),'*');
plot3(radar0(1),radar0(2),radar0(3),'*');
plot3(target(1),target(2),target(3),'>');