complexdynamickalman.m

一个简单的kalman filter 滤波程序

% kalman filter with complex dynamic model

%alpha=forgetting factor(alpha>=1)
%duration=length of simulation(seconds)
%dt=step size(seconds)

clear all
clc

measnoise=10;%position measurement noise(m)
accelnoise=0.5;%acceleration noise(m/sec^2)
dt=0.2;
duration=100;
alpha=1;
ss=4;%state size
os=2;%observation size

a=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];%transition matrix
c=[1 0 0 0 ;0 1 0 0];
x=[0;0;0;0];
xhat=x;%initial state estimation

Q=0.0001*eye(ss);%process noise covariance
pest=Q;%initial estimation covariance
R=100*eye(os);

%set up the size of the innovations vector
Inn=zeros(size(R));

pos=[];%true position array
poshat=[];%estimated position array
posmeas=[];%measured position array
processnoise=[];%process noise array
Measnoise=[];%measurement noise array
gaink=[];%kalman filter gain
Innovation=[];

for t=0:dt:duration,
    
    %simulate the process
processNoise=accelnoise*[(dt^2/2)*randn;(dt^2/2)*randn;dt*randn;dt*randn];
x=[10*sin(t);0;0;0];
xest=x;
x=a*x+processNoise;

%simulate the measurement
MeasNoise=measnoise*[randn;randn];
z=c*x+MeasNoise;

%innovation
Inn=z-c*xhat;

%covariance of innotion
s=c*pest*c'+R;

%Gain matrix
K=pest*c'*inv(s);

%state estimate
xhat=xest+K*Inn;

%covariance of prediction error
P=pest-pest*c'*inv(s)*c*pest;
pest=a*P*a'+Q;
xest=a*xhat;

%save some parameters in vectors for plotting later
pos=[pos;x(1)];
posmeas=[posmeas;z(1)];
poshat=[poshat;xest(1)];
processnoise=[processnoise;processNoise(1)];
Measnoise=[Measnoise;MeasNoise(1)];
gaink=[gaink;K(1)];
Innovation=[Innovation;Inn(1)];
end

%plot the results
t=0:dt:duration;
diff=pos-poshat;

figure(1)
plot(t,pos,'r',t,poshat,'k',t,posmeas,'b')
grid on
title('Kalman Filter Performance')
ylabel('Positions(feet)');
xlabel('Time(sec)');

figure(2)
plot(t,pos)
grid on
ylabel('True Position Trajectory (feet)');
xlabel('Time(sec)');

figure(3)
%plot(t,posmeas)
plot(t,Innovation)
grid on
%ylabel('Measured Position  (feet)');
ylabel('Innovation');
xlabel('Time(sec)');

figure(4)
plot(t,poshat)
grid on
ylabel('Estimated Position Trajectory (m)');
xlabel('Time(sec)');

figure(5)
plot(t,processnoise)
grid on
ylabel('process Noise');
xlabel('Time(sec)');

figure(6)
plot(t,Measnoise )
grid on
ylabel('Measurement Noise');
xlabel('Time(sec)');

figure(7)
plot(t,gaink)
grid on
ylabel('Kalman Filter Gain K');
xlabel('Time(sec)');