Jasmine Roberts - MIT Fab Lab

Kalman Filter

```
(650) 868-7414.

%%Code for Generating required signal and noise:
t = 0:1:500;
a=4*sin(0.01*t);
y=sin(0.1*t+a);
subplot(2,1,1)
plot(t,y)
title('Periodic Sin Wave')
noise = 0.1*randn(size(t));
subplot(2,1,2)
plot(noise);

%%Code for noisy signal:
t = 0:1:500;
noise = 0.1*randn(size(t));
a=4*sin(0.01*t);
y=sin(0.1*t+a)+noise;
subplot(2,1,1)
plot(t,y);
A=1.;
W=1.;
TS=.1;
ORDER=3;
PHIS1=0.;
PHIS2=0.;
SIGX=1.;
T=0.;
S=0.;
H=.001;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT(1,1)=SIGX^2;
PHIH=0.;
WH=2.;
AH=3.;
P(1,1)=0.;
P(2,2)=(W-WH)^2;
P(3,3)=(A-AH)^2;
XT=0.;
XTD=A*W;
count=0;
while T<=20.
XTOLD=XT;
XTDOLD=XTD;
XTDD=-W*W*XT;
XT=XT+H*XTD;
XTD=XTD+H*XTDD;
T=T+H;
XTDD=-W*W*XT;
XT=.5*(XTOLD+XT+H*XTD);
XTD=.5*(XTDOLD+XTD+H*XTDD);
S=S+H;
if S>=(TS-.00001)
S=0.;
PHI(1,1)=1.;
PHI(1,2)=TS;
PHI(2,2)=1.;
PHI(3,3)=1.; % fundamental and process noise matrix
Q(1,1)=TS*TS*TS*PHIS1/3.;
Q(1,2)=.5*TS*TS*PHIS1;
Q(2,1)=Q(1,2);
Q(2,2)=PHIS1*TS;
Q(3,3)=PHIS2*TS;
PHIB=PHIH+WH*TS;
HMAT(1,1)=AH*cos(PHIB);
HMAT(1,2)=0.;
HMAT(1,3)=sin(PHIB);
PHIT=PHI';
HT=HMAT';
PHIP=PHI*P;
PHIPPHIT=PHIP*PHIT;
M=PHIPPHIT+Q;
HM=HMAT*M;
HMHT=HM*HT;
HMHTR=HMHT+RMAT;
HMHTRINV(1,1)=1./HMHTR(1,1);
MHT=M*HT;
K=MHT*HMHTRINV;
KH=K*HMAT;
IKH=IDNP-KH;
P=IKH*M;
XTNOISE=SIGX*randn; % Extended kalman filter
XTMEAS=XT+XTNOISE;
RES=XTMEAS-AH*sin(PHIB);
PHIH=PHIB+K(1,1)*RES;
WH=WH+K(2,1)*RES;
AH=AH+K(3,1)*RES;
ERRPHI=W*T-PHIH; % Compute errors in estimate
SP11=sqrt(P(1,1));
ERRW=W-WH;
SP22=sqrt(P(2,2));
ERRA=A-AH;
SP33=sqrt(P(3,3));
XTH=AH*sin(PHIH);
XTDH=AH*WH*cos(PHIH);
SP11P=-SP11;
SP22P=-SP22;
SP33P=-SP33;
count=count+1;
ArrayT(count)=T;
ArrayW(count)=W;
ArrayWH(count)=WH;
ArrayA(count)=A;
ArrayAH(count)=AH;
ArrayERRPHI(count)=ERRPHI;
ArraySP11(count)=SP11; % Save some data as arrays for plotting and writing files
ArraySP11P(count)=SP11P;
ArrayERRW(count)=ERRW;
ArraySP22(count)=SP22;
ArraySP22P(count)=SP22P;
ArrayERRA(count)=ERRA;
ArraySP33(count)=SP33;
ArraySP33P(count)=SP33P;
end
end
figure
plot(ArrayT,ArrayW,ArrayT,ArrayWH),grid % Plotting states and their estimates
xlabel('Time (Sec)')
ylabel('Frequency (R/S)')
axis([0 20 0 2])
figure
plot(ArrayT,ArrayA,ArrayT,ArrayAH),grid
xlabel('Time (Sec)')
ylabel('Amplitude')
axis([0 20 0 3])

%%Q1;
nlen=20;
a=1;
h=3;
Q=0.01;
R=2;
x=zeros(1,nlen);
z=zeros(1,nlen);
xapriori=zeros(1,nlen);
xaposteriori=zeros(1,nlen);
residual=zeros(1,nlen);
papriori=ones(1,nlen);
paposteriori=ones(1,nlen);
k=zeros(1,nlen);

w1=randn(1,nlen);
v1=randn(1,nlen);
w=w1*sqrt(Q);
v=v1*sqrt(R);
x_0=1.0;

xaposteriori_0=1.5;
paposteriori_0=1;

x(1)=a*x_0+w(1);
z(1)=h*x(1)+v(1);
xapriori(1)=a*xaposteriori_0;
residual(1)=z(1)-h*xapriori(1);
papriori(1)=a*a*paposteriori_0+Q;

k(1)=h*papriori(1)/(h*h*papriori(1)+R);
paposteriori(1)=papriori(1)*(1-h*k(1));
xaposteriori(1)=xapriori(1)+k(1)*residual(1);

for j=2:nlen,

x(j)=a*x(j-1)+w(j);
z(j)=h*x(j)+v(j);
xapriori(j)=a*xaposteriori(j-1);
residual(j)=z(j)-h*xapriori(j);
papriori(j)=a*a*paposteriori(j-1)+Q;
k(j)=h*papriori(j)/(h*h*papriori(j)+R);
paposteriori(j)=papriori(j)*(1-h*k(j));
xaposteriori(j)=xapriori(j)+k(j)*residual(j);
end

j=1:nlen;
subplot(221);
h1=stem(j+0.25,xapriori,'b');
hold on
h2=stem(j+0.5,xaposteriori,'g');
h3=stem(j,x,'r');
hold off
legend([h1(1) h2(1) h3(1)],'a priori','a posteriori','exact');
title('State with a priori and a posteriori elements');
ylabel('State, x');
xlim=[0 length(j)+1];
set(gca,'XLim',xlim);
subplot(222);
h1=stem(j,papriori,'b');
hold on;
h2=stem(j,paposteriori,'g');
hold off
legend([h1(1) h2(1)],'a priori','a posteriori');
title('Calculated a priori and a posteriori covariance');
ylabel('Covariance');
set(gca,'XLim',xlim);
subplot(223);
h1=stem(j,x-xapriori,'b');
hold on
h2=stem(j,x-xaposteriori,'g');
hold off
legend([h1(1) h2(1)],'a priori','a posteriori');
title('Actual a priori and a posteriori error');
ylabel('Errors');
set(gca,'XLim',xlim);

%%Plot kalman gain, k
subplot(224);
h1=stem(j,k,'b');
legend([h1(1)],'kalman gain');
title('Kalman gain');
ylabel('Kalman gain, k');
set(gca,'XLim',xlim);

```