lqed.m

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

function [l,m,p,e] = lqed(a,g,c,q,r,Ts)
%LQED   Discrete linear quadratic estimator design from continuous 
%       cost function.
%	[L,M,P,E] = LQED(A,G,C,Q,R,Ts) calculates the Kalman gain matrix L
%	that minimizes the discrete estimation error equivalent to the 
%	estimation error from the continuous system:
%	         .
%	         x = Ax + Bu + Gw
%                y = Cx + Du +  v
%
%	with process and measurement noise:
%		E{w} = E{v} = 0,  E{ww'} = Q,  E{vv'} = R,  E{wv'} = 0
%
%	Also returned is the discrete Riccati solution M, the estimate
%	error covariance after the measurement update P, and the discrete
%	closed loop loop eigenvalues E = EIG(Ad-Ad*L*Cd).
%
%	The gain matrix is determined by discretizing the continuous plant
%	(A,B,C,D) and continuous covariance matrices (Q,R) using the 
%	sample time Ts and the zero order hold approximation. The gain 
%	matrix is then calculated using DLQE.
%
%	See also: C2D, LQRD, DLQE, and LQE.

%	Clay M. Thompson 7-18-90
%	Copyright (c) 1986-93 by the MathWorks, Inc.

% Reference: This routine is based on the routine DISRW.M by Franklin, 
% Powell and Workman and is described on pp. 454-455 of "Digital Control
% of Dynamic Systems".

error(nargchk(6,7,nargin));
error(abcdchk(a,g,c));
[nx,nu] = size(g);
[ny,nx] = size(c);

[nq,mq] = size(q);
if (mq ~= nq) | (nu ~= mq), error('G and Q must be consistent.'); end
[nr,mr] = size(r);
if (mr ~= nr) | (ny ~= mr), error('C and R must be consistent.'); end

% Check if q is positive semi-definite and symmetric
if any(eig(q) < -eps) | (norm(q'-q,1)/norm(q,1) > eps)
  disp('Warning: Q is not symmetric and positive semi-definite');
end
% Check if r is positive definite and symmetric
if any(eig(r) <= -eps) | (norm(r'-r,1)/norm(r,1) > eps)
  disp('Warning: R is not symmetric and positive definite');
end

% Discretize the state-space system.
[ad,gd] = c2d(a,g,Ts);

% --- Compute discrete equivalent of continuous noise ---
Za = zeros(nx); 
M = [ -a  g*q*g'
      Za   a'   ];
phi = expm(M*Ts);
phi12 = phi(1:nx,nx+1:2*nx);
phi22 = phi(nx+1:2*nx,nx+1:2*nx);
Qd = phi22'*phi12;
Qd = (Qd+Qd')/2;		% Make sure Qd is symmetric

Rd = r/Ts;

% Design the gain matrix using the discrete plant and discrete cost function
[l,m,p,e] = dlqe(ad,eye(nx),c,Qd,Rd);