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);