%擴(kuò)展卡爾曼濾波　偽距法定位
format long
clear
clc

%假設(shè)真實(shí)的P坐標(biāo)與tu值，以作參考
c=3.0e8;              %光速
pu=[1,2,3];           %假定用戶(hù)接收機(jī)的真實(shí)坐標(biāo)
tu=5e-3;              %假定真實(shí)的鐘差

%4顆衛(wèi)星真實(shí)位置
s1=[5000,4000,6000];
s2=[-1000,5000,3000];
s3=[1000,3000,3000];
s4=[-3000,4500,3000];

%偽距p(沒(méi)有噪聲影響)
t=s1-pu; p(1)=sqrt(sum(t.*t))+c*tu;
t=s2-pu; p(2)=sqrt(sum(t.*t))+c*tu;
t=s3-pu; p(3)=sqrt(sum(t.*t))+c*tu;
t=s4-pu; p(4)=sqrt(sum(t.*t))+c*tu;

MAX=100;%數(shù)據(jù)量個(gè)數(shù)
RD=randn(MAX,4)/100;%下面程序中將用到，產(chǎn)生隨機(jī)噪聲，使偽距顯得更“實(shí)際”


%卡爾曼濾波器初始值
T(1).pu=[10,20,30]; T(1).tu=6e-3;
T(1).X = ( [T(1).pu,T(1).tu] )';  %用戶(hù)位置初始值、接收機(jī)鐘差初始值    T(k).X_ ( 4 X 1 矩陣)
T(1).P = 0;

R=0.01*eye(4);     % 4 X 4 矩陣
Q=1e-5*eye(4);     % 4 X 4 矩陣


for k=2:1:MAX
    %帶噪聲的偽距
    T(k).Z  = ( p+RD(k,:) )';  %觀測(cè)量  4 X 1 矩陣
    T(k).X_ = T(k-1).X ;      %狀態(tài)量
    T(k).pu = [T(k).X_(1),T(k).X_(2),T(k).X_(3)];
    T(k).tu = T(k).X_(4);
    
    %近似偽距p_
    t=s1-T(k).pu;     r_(1)=sqrt(sum(t.*t));    p_(1)=r_(1)+c*T(k).tu;   
    t=s2-T(k).pu;     r_(2)=sqrt(sum(t.*t));    p_(2)=r_(2)+c*T(k).tu;
    t=s3-T(k).pu;     r_(3)=sqrt(sum(t.*t));    p_(3)=r_(3)+c*T(k).tu;
    t=s4-T(k).pu;     r_(4)=sqrt(sum(t.*t));    p_(4)=r_(4)+c*T(k).tu;
    T(k).Z_ = p_' ;
    
    %構(gòu)造H矩陣
    H=c*ones(4,4);
    for i=1:3
        H(1,i)=-(s1(i)-T(k).pu(i))/r_(1);     %a1(i)
        H(2,i)=-(s2(i)-T(k).pu(i))/r_(2);     %a2(i)
        H(3,i)=-(s3(i)-T(k).pu(i))/r_(3);     %a3(i)
        H(4,i)=-(s4(i)-T(k).pu(i))/r_(4);     %a4(i)
    end 
    T(k).H=H;
    
    T(k).P_ = T(k-1).P + Q ; 
    T(k).Kg = T(k).P_ * T(k).H' / ( T(k).H*T(k).P_*T(k).H'+ R ) ;     %卡爾曼增益
    T(k).X  = T(k).X_ + T(k).Kg * ( T(k).Z - T(k).Z_ );               %觀測(cè)變量的更新
    T(k).P  = (1 - T(k).Kg * T(k).H) * T(k).P_ ;     
    
end  %end of for

%作圖
figure(1);
clf reset;
for k=1:MAX;
    x(k)=T(k).pu(1);
    y(k)=T(k).pu(2);
    z(k)=T(k).pu(3);
end
k=1:MAX;  
plot3(x(k),y(k),z(k),'r.');
axis on;
grid on;