kalmanexpri.m

The Kalman filter is a set of mathematical equations that provides an efficient computational [recu

clc;
clear all;
format long;
t = 0:0.01:10;
y=cos(2*pi*t);
modtrans = sin(2*pi*t*5 +(3)*sin(2*pi*1*t));
figure; plot(t,modtrans); title('Transmitted FM modulated signal');
xlabel('time');
ylabel('Amplitude');
autocorr=xcorr(modtrans,modtrans);
clutter=[0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i
   0.0043+1.44i
   0.00+1.008i
   0.005+1.006i
   0.0023+1.002i
   0.0034+1.078i
   0.0055+1.034i
   0.0078+1.089i
   0.0045+1.045i];
for i=551:1001
    clutter(i)=0;
end
delay=0.01;
for i=1:101
    rxmodtrans = sin(2*pi*(t+delay)*5 +(3)*sin(2*pi*1*(t+delay)));
%     rxmodtrans= rxmodtrans + .001*rand(1,1001);
    rxmodtrans1= rxmodtrans + 10*randn(size( rxmodtrans));%    src(1,1001,[0 .2 .25 ; .05 .55 .40]);
    clutdelay=delay+rand(1,1);
%     rxmodtrans = rxmodtrans + sin(2*pi*(t+clutdelay)*5 +(3)*sin(2*pi*1*(t+clutdelay)));
    rxmodtrans2 = rxmodtrans1+clutter';
    crosscorr=xcorr(modtrans,rxmodtrans);
    crosscorr1=xcorr(modtrans,rxmodtrans2);
    [xauto,xautoi]=max(autocorr);
    [xcross,xcrossi]=max(crosscorr);
     [xcross1,xcrossi1]=max(crosscorr1);
    td(i)=abs((xcrossi-xautoi))*.01;
    Range(i)=(3*10^8)*td(i)/2;
     td1(i)=abs((xcrossi1-xautoi))*.01;
    Range1(i)=(3*10^8)*td1(i)/2;
    d(i)=delay;
    delay=delay+.005;
    RAN(i)=(d(i)*1.5*10^8);
    
end
for i=1:101
    az(i)=30+.1;
end

% RANGE = awgn(Range,35,'measured');
RANGE = Range;
R=50^2;
T=5;
%UVEL(1)=nm2km(1500);
UVEL(1)=1500;
A=[1 T;0 1];    
atrans = A';
H=[1 0 ];
htrans = H';
%Q=.7;
Q=[(T^6)/36 (T^5)/12;(T^5)/12 (T^4)/4];
URANGE(1)=1.5*10^6;
 tou(2)=0.016;
UERRORCOVAR=[248 14;14 248];
e1(1)=.003;
e1(2)=.008;
delay=0.0005;
%  tic;
for a= 2 : 101

    PRANGE(a) = URANGE(a-1)+(T * 150000);
    PVEL(a)=UVEL(a-1);
    %z(a)=awgn(RANGE(a),42,'measured');
    z(a)=RANGE(a)+normrnd(0,500);
            
    c=A*UERRORCOVAR;
    PERRORCOVAR=(c*A')+Q;
          
    for j=1:2
             perrorco(a,j)=PERRORCOVAR(j,1);
             perrorco(a+1,j)=PERRORCOVAR(j,2);
    end     
  
% %   t1=exp(delay/.00125); 
%     RR=t1*R;
%     c=PERRORCOVAR*H';
%     d1=H*c;
%     e=(d1+RR); 
%     D=inv(e);
% t1=exp(.005/.0026);
%      t1=exp(td(a-1)/.02);
%      t2(a)=t1;
%     if(td(a)>td(a-1))
%         t2(a)=t2(a-1);
%     end
%     RR=t1*R;
 RR=R;
    c=PERRORCOVAR*H';
    d1=H*c;
    e=(d1+RR); 
    D=inv(e);

    for i=1:2
             TEMPKALGAIN(i,1)=0;
             TEMPKALGAIN(i,1)=TEMPKALGAIN(i,1)+(c(i,1)*D(1,1));
             kalgain(i,a-1)=TEMPKALGAIN(i,1);           
    end
        
    %temp=H*PRANGE(a);
    temp1=(z(a)-PRANGE(a));
    %plot(a,temp1,'k'); hold on;
    URANGE(a)=PRANGE(a)+(kalgain(1,a-1)*temp1);
    UVEL(a)=PVEL(a)+(kalgain(2,a-1)*temp1)/T;
    

    temp=kalgain(:,a-1)*H;
    temp1=eye(2)-temp;
%     UERRORCOVAR=t1*temp1*PERRORCOVAR;  
    UERRORCOVAR=temp1*PERRORCOVAR;
     tou(a)=URANGE(a)*2/(3*10^8);
%      e1(a+1)=toc;
    end