%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%?程序功能：三基站觀測距離EKF與LS算法比較分析
clc;clear;close?all;
T=1;%?采樣時間間隔
N=50/T;%?總的采樣次數
F=[10T0（T^2）/20;010T0（T^2）/2;0010T0;00010T;000010;000001];%?狀態轉移矩陣
delta_q=1e-8;%?如果增大這個參數，目標真實軌跡就是曲線了
Q=delta_q*eye（6）;%?過程噪聲方差?
W=sqrt（Q）*randn（6N）;
delta_r=1e-2;
R=delta_r*eye（3）;%?觀測噪聲方差
V=sqrtm（R）*randn（3N）;
Node_number=3;%?基站個數
Station=[01010;0010];%?3個基站位置初始化
X=zeros（6N）;
Z=zeros（3N）;
X（:1）=[0;0;0.15;0.15;0.002;0.002];%?目標狀態位置、速度、加速度初始化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%基于3個基站觀測距離的目標跟蹤LS狀態估計
for?k=1:N-1
????X（:k+1）=F*X（:k）+W（:k）;%?狀態方程建模，模擬受到過程噪聲的污染
end
for?t=1:N
????for?i=1:Node_number
????????d（i:）=Dist（X（:t）Station（:i））;%?多個觀測站對t時刻目標距離測量
????end
????Z（:t）=d+V（:t）;%?觀測方程的建模，模擬受到觀測噪聲的污染
????for?i=2:Node_number
????????H（i-1:）=[2*（Station（1i）-Station（11））2*（Station（2i）-Station（21））];
????????b（i-1:）=[Z（1t）^2-Z（it）^2+Station（1i）^2+Station（2i）^2-Station（11）^2-Station（21）^2];
????end??
????Xls（:t）=inv（H‘*H）*H‘*b;%?最小二乘法得到目標當前時刻的位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%基于3個基站觀測距離的目標跟蹤EKF狀態估計
P0=eye（6）;%?協方差矩陣初始化
Xekf（:1）=X（:1）;%?卡爾曼濾波狀態初始化
for?t=2:N?
????Xn=F*Xekf（:t-1）;%?狀態預測
????Zn=[Dist（XnStation（:1））;
????????Dist（XnStation（:2））;
????????Dist（XnStation（:3））;
???????];%?觀測預測
????P1=F*P0*F‘+Q;%?誤差協方差預測
????H=[（Xn（1）-Station（11））/Zn（1）（Xn（2）-Station（21））/Zn（1）0000;
???????（Xn（1）-Station（12））/Zn（2）（Xn（2）-Station（22））/Zn（2）0000;
???????（Xn（1）-Station（13））/Zn（3）（Xn（2）-Station（23））/Zn（3）0000;
???????];%求雅可比矩陣，即為所求一階近似
????K=P1*H‘/（H*P1*H‘+R）;%卡爾曼濾波增益
????Xekf（:t）=Xn+K*（Z（:t）-Zn）;%狀態更新
??