dlqe.m

数字通信第四版原书的例程

function [l,m,p,e] = dlqe(a,g,c,q,r,t)
%DLQE	Discrete linear quadratic estimator design for the system:
%		x[n+1] = Ax[n] + Bu[n] + Gw[n]	  {State equation}
%		z[n]   = Cx[n] + Du[n] +  v[n]	  {Measurements}
%	with process noise and measurement noise covariances:
%		E{w} = E{v} = 0,  E{ww'} = Q,  E{vv'} = R,  E{wv'} = 0
%	L = DLQE(A,G,C,Q,R) returns the gain matrix L such that the 
%	discrete, stationary Kalman filter with time and observation 
%	update equations:
%	   _         *               *      _                _
%	   x[n+1] = Ax[n] + Bu[n]    x[n] = x[n] + L(z[n] - Cx[n] - Du[n])
%	produces an LQG optimal estimate of x.  The estimator can be 
%	formed using DESTIM.
%
%	[L,M,P,E] = DLQE(A,G,C,Q,R) returns the gain matrix L, the Riccati
%	equation solution M, the estimate error covariance after the 
%	measurement update:          *    *
%                             P = E{[x-x][x-x]'}
%	and the closed-loop eigenvalues of the estimator, E=EIG(A-A*L*C).
%
%	[L,M,P,E] = DLQE(A,G,C,Q,R,N) solves the discrete est. problem
%	when the process and sensor noise is correlated: E{wv'} = N.
%
%	See also: DLQEW, LQED and DESTIM.

%	J.N. Little 4-21-85
%	Revised Clay M. Thompson  7-16-90
%	Copyright (c) 1986-93 by the MathWorks, Inc.

error(nargchk(5,6,nargin));

% Use DLQR to calculate estimator gains using duality
if nargin==5
  [k,s,e] = dlqr(a',c',g*q*g',r);
  m = s';
  l = (m*c')/(c*m*c' + r);
else
  [k,s,e] = dlqr(a',c',g*q*g',r,g*t);
  m = s';
  % Because of the notation used for discrete Kalman filters, the Kalman
  % gain matrix shows up in the observation update equation.  When designing
  % with cross terms, this requires that the A matrix be inverted when 
  % computing the L matrix.
  if rcond(a)<eps, disp('Warning: The A matrix must be non-singular for Kalman gain calculation.'); end
  l = (m*c'+a\g*t)/(c*m*c' + r);
  a = a-g*t/r*c;
  q = q-t/r*t';
end  
p = a\(m-g*q*g')/a';