kalman filtering -凯发k8网页登录

steady-state kalman filter

consider the following discrete plant with gaussian noise w on the input and measurement noise v on the output:

the goal is to design a kalman filter to estimate the true plant output yt[n]=y[n]-v[n] based on the noisy measurements y[n]. this steady-state kalman filter uses the following equations for this estimation.

time update:

measurement update:

here,

(these update equations describe a current type estimator. for information about the difference between current estimators and delayed estimators, see .)

design the filter

you can use the kalman function to design this steady-state kalman filter. this function determines the optimal steady-state filter gain m for a particular plant based on the process noise covariance q and the sensor noise covariance r that you provide. for this example, use the following values for the state-space matrices of the plant.

a = [1.1269   -0.4940    0.1129 
     1.0000         0         0 
          0    1.0000         0];
b = [-0.3832
      0.5919
      0.5191];
c = [1 0 0];
d = 0;

for this example, set g=b, meaning that the process noise w is additive input noise. also, set h=0, meaning that the input noise w has no direct effect on the output y. these assumptions yield a simpler plant model:

when h = 0, it can be shown that my=cmx (see ). together, these assumptions also simplify the update equations for the kalman filter.

time update:

measurement update:

to design this filter, first create the plant model with an input for w. set the sample time to -1 to mark the plant as discrete (without a specific sample time).

ts = -1;
sys = ss(a,[b b],c,d,ts,'inputname',{'u' 'w'},'outputname','y');  % plant dynamics and additive input noise w

the process noise covariance q and the sensor noise covariance r are values greater than zero that you typically obtain from studies or measurements of your system. for this example, specify the following values.

q = 2.3; 
r = 1; 

use the kalman command to design the filter.

[kalmf,l,~,mx,z] = kalman(sys,q,r);

this command designs the kalman filter, kalmf, a state-space model that implements the time-update and measurement-update equations. the filter inputs are the plant input u and the noisy plant output y. the first output of kalmf is the estimate yˆ of the true plant output, and the remaining outputs are the state estimates xˆ.

for this example, discard the state estimates and keep only the first output, yˆ.

kalmf = kalmf(1,:);

use the filter

to see how this filter works, generate some data and compare the filtered response with the true plant response. the complete system is shown in the following diagram.

to simulate this system, use a sumblk to create an input for the measurement noise v. then, use connect to join sys and the kalman filter together such that u is a shared input and the noisy plant output y feeds into the other filter input. the result is a simulation model with inputs w, v, and u and outputs yt (true response) and ye (the filtered or estimated response yˆ). the signals yt and ye are the outputs of the plant and the filter, respectively.

sys.inputname = {'u','w'};
sys.outputname = {'yt'};
vin = sumblk('y=yt v');
kalmf.inputname = {'u','y'};
kalmf.outputname = 'ye';
simmodel = connect(sys,vin,kalmf,{'u','w','v'},{'yt','ye'});

to simulate the filter behavior, generate a known sinusoidal input vector.

t = (0:100)';
u = sin(t/5);

generate process noise and sensor noise vectors using the same noise covariance values q and r that you used to design the filter.

rng(10,'twister');
w = sqrt(q)*randn(length(t),1);
v = sqrt(r)*randn(length(t),1);

finally, simulate the response using lsim.

out = lsim(simmodel,[u,w,v]);

lsim generates the response at the outputs yt and ye to the inputs applied at w, v, and u. extract the yt and ye channels and compute the measured response.

yt = out(:,1);   % true response
ye = out(:,2);  % filtered response
y = yt   v;     % measured response

compare the true response with the filtered response.

clf
subplot(211), plot(t,yt,'b',t,ye,'r--'), 
xlabel('number of samples'), ylabel('output')
title('kalman filter response')
legend('true','filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('number of samples'), ylabel('error')
legend('true - measured','true - filtered')

as the second plot shows, the kalman filter reduces the error yt - y due to measurement noise. to confirm this reduction, compute the covariance of the error before filtering (measurement error covariance) and after filtering (estimation error covariance).

measerr = yt - y;
measerrcov = sum(measerr.*measerr)/length(measerr)

measerrcov = 0.9871

esterr = yt - ye;
esterrcov = sum(esterr.*esterr)/length(esterr)

esterrcov = 0.3479

time-varying kalman filter design

the previous design assumed that the noise covariances do not change over time. a time-varying kalman filter can perform well even when the noise covariance is not stationary.

the time-varying kalman filter has the following update equations. in the time-varying filter, both the error covariance p[n] and the innovation gain mx[n] can vary with time. you can modify the time and measurement update equations to account for time variation as follows. (see for more detail on these expressions.)

time update:

measurement update:

you can implement a time-varying kalman filter in simulink® using the block. for an example demonstrating the use of that block, see state estimation using time-varying kalman filter. for this example, implement the time-varying filter in matlab®.

to create the time-varying kalman filter, first, generate the noisy plant response. simulate the plant response to the input signal u and process noise w defined previously. then, add the measurement noise v to the simulated true response yt to obtain the noisy response y. in this example, the covariances of the noise vectors w and v do not change with time. however, you can use the same procedure for nonstationary noise.

yt = lsim(sys,[u w]);   
y = yt   v;         

next, implement the recursive filter update equations in a for loop.

p = b*q*b';         % initial error covariance
x = zeros(3,1);     % initial condition on the state
ye = zeros(length(t),1);
ycov = zeros(length(t),1); 
errcov = zeros(length(t),1); 
for i=1:length(t)
  % measurement update
  mxn = p*c'/(c*p*c' r);
  x = x   mxn*(y(i)-c*x);   % x[n|n]
  p = (eye(3)-mxn*c)*p;     % p[n|n]
  ye(i) = c*x;
  errcov(i) = c*p*c';
  % time update
  x = a*x   b*u(i);        % x[n 1|n]
  p = a*p*a'   b*q*b';     % p[n 1|n] 
end

compare the true response with the filtered response.

subplot(211), plot(t,yt,'b',t,ye,'r--')
xlabel('number of samples'), ylabel('output')
title('response with time-varying kalman filter')
legend('true','filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('number of samples'), ylabel('error')
legend('true - measured','true - filtered')

the time-varying filter also estimates the output covariance during the estimation. because this example uses stationary input noise, the output covariance tends to a steady-state value. plot the output covariance to confirm that the filter has reached a steady state.

figure
plot(t,errcov) 
xlabel('number of samples'), ylabel('error covariance'),

from the covariance plot, you can see that the output covariance reaches a steady state in about five samples. from then on, the time-varying filter has the same performance as the steady-state version.

as in the steady-state case, the filter reduces the error due to measurement noise. to confirm this reduction, compute the covariance of the error before filtering (measurement error covariance) and after filtering (estimation error covariance).

measerr = yt - y;
measerrcov = sum(measerr.*measerr)/length(measerr)

measerrcov = 0.9871

esterr = yt - ye;
esterrcov = sum(esterr.*esterr)/length(esterr)

esterrcov = 0.3479

finally, when the time-varying filter reaches steady state, the values in the gain matrix mxn match those computed by kalman for the steady-state filter.

mx,mxn

mx = 3×1
    0.5345
    0.0101
   -0.4776

mxn = 3×1
    0.5345
    0.0101
   -0.4776