ekf_example.m

ekf扩展卡尔曼滤波,效果比较好extended kalman filter

clear
close all
Q = 10;
R = 0.1;
x(1) = 0;
for m = 2:101
    x(m) = x(m-1)/2 + 25*x(m-1)/(1+x(m-1)*x(m-1)) + 8*cos(1.2*x(m-1))+normrnd(0,Q);
%     x(m) = x(m-1)/2 + 25*x(m-1)/(1+x(m-1)*x(m-1))+normrnd(0,Q);
end
figure;
plot(0:100,x)
axis([0,100,-50,50]);
grid;

a = x;

for m =1:101
    y(m) = x(m)*x(m)/20+normrnd(0,R);
end
figure;
plot(0:100,y);
% axis([0,100,-50,50]);
grid;

x(1) = 0.1;
p(1) = 0.5*sqrt(3.1415926)*exp(-0.25*x(1)*x(1));
for i = 2:101
    [x(i),p(i)] = EKF_ex1(x(i-1),p(i-1),y(i-1),Q,R);
end
figure;
plot(0:100,a,'.',0:100,x,'r');
axis([0,100,-50,50]);
grid;

number = 100;
x = normrnd(0,2,1,number);
x(1,:) = x;
for i = 1:number
    w(1,i) = 0.5*sqrt(3.1415926)*exp(-0.25*x(1,i)*x(1,i));
end
for i = 2:101
    [x(i,:),w(i,:)] = Bootstrap_ex1(x(i-1,:),w(i-1,:),y(i-1),number,Q,R);
    meanx(i) = 0;
    for m = 1:number
        meanx(i) = meanx(i)+w(i,m)*x(i,m);
    end
end
figure;
plot(0:100,a,'.',0:100,meanx,'r');
axis([0,100,-50,50]);
grid;