clear all
clc
deta_t=1; %sec
beta=10;
t=1*deta_t:deta_t:100;
%%simulation of terrian database
h=50*sin(t/60*deta_t*2*pi)+100; %True Terrain Profile
h_dma=h+6*randn(1,100/deta_t); %digital database
h_rad=3*randn(1,100/deta_t)+110; %radar altitude,110 m above groud
h_msl=h+6*randn(1,100/deta_t)+110+50; %altitude measured by Navigation system,110 m above groud,and 50m constant error
% plot(t,h_dma,'r');hold on
% plot(t,h_dma,'r');
% plot(t,h_dma,'r');
%Discrete-time model
H=[1 1;1 0];
psi=[1 0;0 exp(deta_t*beta)];
%Initial parameters of kalman filter
Q=[36 0;0 225];
R=[9 0;0 36];
Pk_1=[10 0;0 24];
Xk_1=[0 0]';
fout=fopen('data2.txt','w');
for i=1*deta_t:deta_t:100
Z=[h_msl(int8(i/deta_t))-h_dma(int8(i/deta_t));h_rad(int8(i/deta_t))];
%predictor
Xk_k_1=psi*Xk_1; %state transition
Pk_k_1=psi*Pk_1*psi'+Q; %prediction convariance
Pk_1=Pk_k_1;
Xk_1=Xk_k_1;
%estimator
K=Pk_k_1*H'*inv(H*Pk_k_1*H'+R);%Kalman gain
Xk=Xk_k_1+K*(Z-H*Xk_k_1); %state estimator
Pk=(eye(2)-K*H)*Pk_k_1; %estimate covariance
Pk_1=Pk;
Xk_1=Xk;
fprintf(fout,'%12.8f %12.8f %12.8f %12.8f %12.8f %12.8f %12.8f %12.8f %12.8f\n',sqrt(Pk(1,1)),sqrt(Pk(2,2)),Xk(1),Xk(2),h(int8(i/deta_t)),h_rad(int8(i/deta_t))+h(int8(i/deta_t)),h_dma(int8(i/deta_t)),h_msl(int8(i/deta_t)),h_msl(int8(i/deta_t))-Xk(2));
end
fclose(fout);
