ISU EE 573 SPRING 1998 Matlab Programs
%Program Name: wsim2.m
% PURPOSE: To investigate the influence of delt on the wiener
% filter estimat of a 2nd order underdamped system excited by white noise
% plus additive Gauss Markov noise. This program is a modification of wsim
% in that it assumes the data time frame is fixed. Consequently,
% the sampled data size will increase with increasing sample rate.
% =======================================================
% ********* SIGMAL SECTION ************
% SPECIFY 2ND ORDER SYSTEM PARAMETERS
zeta=0.01; %analog system damping ratio
wn=10; %analog system undamped natural frequency
% ********* NOISE SECTION *********************
% Specify Gauss-Markov Noise bandwidth to be fixed @ 10 Wn
wb=10*wn;
Sxx=[];
Suu=[];
delt=[];
AR1=[];
ARMA21=[];
% COMPUTE DIGITAL SYSTEM PSD FOR Wsamp = 10, 100 & 1000 times Wn
nptsmax = 512*100; % max data size
frac=0.1; % fraction of points to be plotted
for m=1:3
ws=wn*10^m; %sampling frequency is a min of 10 times wn
npts = 512*10^(m-1); % the data size is proportional to the sample rate
vn=wn/ws;
deltm=2*pi/ws;
delt=[delt deltm];
% SIGNAL PSD
alpha=exp(-2*pi*zeta*vn);
beta=2*pi*vn*(1-zeta^2)^0.5;
a1=alpha*sin(beta);% ARMA(2,1) signal numerator parameter
b1=-2*alpha*cos(beta); % signal denominator parameter
b2=alpha^2; % signal denominator parameter
ARMA21=[ARMA21;[a1 b1 b2]];
A=(deltm*abs(fft([0 a1],npts))).^2;
A=A(1:npts/2);
B=(abs(fft([1 b1 b2],npts))).^2;
B=B(1:npts/2);
Sxxm=A./B;
Sxxm=Sxxm/Sxxm(1); % normalize psd to 0dB at dc
Sxx=[Sxx ;[Sxxm Sxxm(npts/2)*ones(1,(nptsmax-npts)/2)]]; % pad psd so all are same length
% NOISE PSD
d1=-exp(-2*pi*wb/ws);% AR(1) noise parameter
AR1=[AR1;d1];
C=(abs(fft([1],npts))).^2;
C=C(1:npts/2);
D=(abs(fft([1 d1],npts))).^2;
D=D(1:npts/2);
Suum=C./D;
Suum=Suum/Suum(1); % normalize psd to 0dB at dc
Suu=[Suu ; [Suum Suum(npts/2)*ones(1,(nptsmax-npts)/2)]]; % pad psd so all are same length
end
% ======================================================
fvec=0:1/nptsmax:0.5-1/nptsmax;
figure(1)
nn=nptsmax*frac;
semilogx(fvec(1:nn),10*log10(Sxx(:,1:nn)))
xlabel('Normalized Frequency (Hz)')
ylabel('Sampled SIGNAL PSD (dB')
title('SIGNAL PSDs for ws=10, 100 and 1000 times wn')
pause
% ===========================
figure(2)
semilogx(fvec(1:nn),10*log10(Suu(:,1:nn)))
xlabel('Normalized Frequency (Hz)')
ylabel('Sampled NOISE PSD (dB')
title('NOISE PSDs for ws=10, 100 and 1000 times wn')
pause
% =============================================
%Compute Wiener Filters
H=Sxx./(Sxx+Suu);
figure(3)
semilogx(fvec(1:nn),10*log10(H(:,1:nn)))
xlabel('Normalized Frequency (Hz)')
ylabel('Sampled WIENER FILTER MAGNITUDE (dB)')
title('WIENER FILTERS for ws=10, 100 and 1000 times wn')
MV/AR program
% Program Name: SC573.m
% =====================================================
% REQUIRED RESIDENT INPUT:
% vector 'ts' containing data
% 'fsamp' = sample frequency associated with ts file
% 'ordsve'=[n1,n2,n3,n4] MV(n) orders
% EXAMPLE:
% ordsve=[5 10 20 40];
%
% ===================================================
% OTHER REQUIRED INPUT PROVIDED BY THIS PROGRAM:
maxorder=max(ordsve);
% max order to be computed
nfft=2048; %number of FFT points for desired freq. resolution
% ***************************************************************
[r,c]=size(ts); % make sure ts is a column vector
if r == 1
ts=ts';
end
ts=ts-mean(ts); % remove mean of ts
%************************************************************
% COMPUTE AUTOCORRELATION SEQUENCE
% ================================
N=length(ts);
r(1)=(ts' * ts)/N;
xvec=[ts ; zeros(N,1)];
for t=1:maxorder
y=[zeros(t,1) ;
xvec(1:(2*N-t))];
r(t+1)=(xvec' * y) / (N-t);
end
%***********************************************************
% COMPUTE LEVINSON POLYNOMIALS
% AS A FUNCTION OF exp(iw)
% ===================================
E2=[];
E2(1)=r(1);
Alpha=1.0;
Aall=[Alpha; zeros(maxorder,1)];
for n=1:maxorder
rflip=fliplr(r(1:n+1));
Alpha=[Alpha; 0.0];
del=rflip * Alpha;
Alpha=Alpha - (del/E2(n)) * flipud(Alpha);
E2(n+1) = E2(n) - (del^2)/E2(n);
Aall=[Aall,[Alpha; zeros(maxorder-n,1)]];
end
Aall=flipud(Aall);
%*******************************************************
% COMPUTE MOD-SQD. FFTS OF POLYNOMIALS
% ===================================
sumphi2=zeros(nfft,1);
for order=0:maxorder
order
phi=(fft(Aall(:,order+1),nfft)) ./ sqrt(E2(order+1));
phimod2=(abs(phi)) .^ 2;
sumphi2=sumphi2 + phimod2;
%****************************
% SEARCH FOR SUMS TO BE SAVED
if order == ordsve(1)
mv1= 1 ./ sumphi2;
ar1= 1 ./ phimod2;
elseif order == ordsve(2)
mv2= 1 ./ sumphi2;
ar2= 1 ./ phimod2;
elseif order == ordsve(3)
mv3= 1 ./ sumphi2;
ar3= 1 ./ phimod2;
elseif order == ordsve(4)
mv4= 1 ./ sumphi2;
ar4= 1 ./ phimod2;
end
end
%****************************
NAr=[ar1 ,ar2, ar3, ar4];
NAr2=NAr(1:nfft/2,:);
NMv=[mv1 ,mv2, mv3, mv4];
NMv2=NMv(1:nfft/2,:);
faxis=0:fsamp/nfft:(fsamp/2)-(fsamp/nfft);
figure(1)
plot(faxis,10*log10(NAr2))
title('AR SPECTRA')
xlabel('FREQUENCY');
ylabel(' dB')
pause
figure(2)
plot(faxis,10*log10(NMv2))
title('MV SPECTRA')
xlabel('FREQUENCY');
ylabel(' dB')
T.V. Sine Generation
% PROGRAM NAME: sinegen.m
%PURPOSE: generate a realization of a t.v. sinusoid+noise
%=======================
% Specification of std. deviations
sea=0.05; % amplitude driving noise sigma
sew=0.05; % angular frequency driving noise sigma
sn=0.707; % additive noise sigma
sea2=sea^2;
sew2=sew^2;
sn2=sn^2;
%=======================
% Other Required Inputs:
npts=2000;
ntot=npts + 500;
a0=1.0; %nominal amplitude
w0=2*pi*0.2; % nominal frequency
%======================
% Generation of white processes
eda=sea*randn(1,ntot);
edw=sew*randn(1,ntot);
% Compute variances of a(t) and w(t)
sa2=sea2/(1-0.9^2);
sw2=sew2/(1-0.95^2);
%======================
% Generation of additive noise process
n=sn*randn(1,npts);
%=====================
% Specify Initial Conditions
da(1)=eda(1);
dw(1)=edw(1);
theta(1)=0;
a(1)=a0;
w(1)=w0;
%====================
% Generate Realization
for t=2:ntot
da(t)=0.9*da(t-1)+eda(t);
a(t)=a0 + da(t);
dw(t)=0.95*dw(t-1)+edw(t);
w(t) = w0 + dw(t);
theta(t) = theta(t-1) + w(t);
end
s=a.*sin(theta);
s=s(501:ntot);
figure(1)
plot(s)
title('T.V. Sinusoid')
pause
z=s + n;
hold on
plot(z,'m')
title('T.V. Sinusoid with & without Noise')
pause
hold off
%=====================
%Plot True Freq. and Ampl.
ftrue=w(501:ntot)/(2*pi);
atrue=a(501:ntot);
figure(2)
plot(ftrue)
title('Actual T.V. Frequency')
xlabel('Time [sec]')
ylabel('Frequency [Hz]')
pause
figure(3)
plot(atrue)
title('Actual T.V. Amplitude')
xlabel('Time [sec]')
ylabel('Amplitude')
T.V. Sine EKF
% PROGRAM NAME: sinekf.m
% PURPOSE: Track a t.v. sinusoid
% REQUIRED INPUT: from program sinegen.m
%======================================
% Model Matrices
I=[1 0 0 ;0 1 0;0 0 1];
Phi=[1 0 0 ; 1 1 0 ; 0 0 1];
Q=[sw2 sw2/2 0 ; sw2/2 sw2/4 0 ; 0 0 sa2];
R=sn2;
%=====================================
% EKF Initial Conditions
xm=[2*pi*0.2;0;1.0];
x=xm;
Pm=Q;
%====================================
% EKF Loop
for k=1:npts
h2=xm(3)*cos(xm(2));
h1=h2*xm(1);
h3=sin(xm(2));
H=[h1 h2 h3];
K=Pm*H'*(H*Pm*H' + R)^(-1);
zm=xm(3)*sin(xm(2));
xhatk=xm + K*(z(k) - zm);
x=[x,xhatk];
P=(I - K*H)*Pm;
xm=Phi*xhatk;
Pm=Phi*P*Phi' + Q;
end
x=x(:,2:npts+1);
fhat=x(1,:)/(2*pi);
ahat=x(3,:);
thetahat=x(2,:);
figure(4)
plot(fhat);
title('EKF Estimate of T.V. Frequency')
xlabel('Time [sec]')
ylabel('Frequency [Hz]')
pause
figure(5)
plot(ahat)
title('EKF Estimate of T.V. Amplitude')
xlabel('Time [sec]')
ylabel('Amplitude')
%=====================
% Reconstruct Signal Estimate
shat=ahat.*sin(thetahat);
figure(6)
plot(shat)
hold
plot(s,'m')
title('Comparison of True and EKF-Estimated Signals')
Model Adaptive KF
% PROGRAM NAME: mmae.m
% Multiple Model Adaptive Estimation- an Example
% AR(1) Parameter Probability Information
%Generate AR(1) realization with a=0.9
x=0;
e=randn(1100,1);
for i=2:1100
x(i)=0.9*x(i-1)+e(i);
end
x=x(101:1100);
a=[0.9;0.3]; % Two possible values for a
pra=[0.5;0.5]; %Assign equal probability to each
R=[1-a(1)^2 ; 1-a(2)^2]; %white noise variances for sigmax=1
xhat=[0;0];
pm=[0.5;0.5];
Xhat=[];
Pmat=[];
K=[];
pr=[];
Pm=[1;1];
for k=1:1000
K=Pm.*(Pm+R).^(-1);
Kmat=[Kmat,K];
xvec=x(k)*[1;1];
p=(2*pi*(Pm+R)).^(-0.5);
p=p.*exp((-0.5*(xvec-xhat).^2)./(Pm+R)).*pm;
pr=[pr,p];
pm=p;
P=[P , ([1;1]-K).*Pm];
xhat=xhat+K.*(xvec-xhat);
Xhat=[Xhat,xhat];
P=([1;1]-K).*Pm;
Pmat=[Pmat,P];
Pm=[a(1)^2;a(2)^2].*P;
xhat=a.*xhat;
end