function Pnew=meP(P,A,B)

% function Pnew=meP(Pold,A,B) : Pnew correction to Pold so that it is consistent with A,B
%
% Pnew is a correction to P which is made consistent with
%
%      X - AXA' = BH + H'B'
%
%      and, in the coordinate system where Pold is the identity,
%      Pnew has the same trace and smallest geometric mean
%      for its eigenvalues, i.e.,
%
%      Pnew = arg max { trace(log(P)) : P is consistent
%             with the requirement of being state covariance}
%
%      the trace of Pnew is set to equal that of Pold in
%      this coordinate system.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global matU vecr

Phlf=sqrtm(P);
Phlfinv=inv(Phlf);
A=Phlfinv*A*Phlf;
B=Phlfinv*B;
Neps=1e-15;
[n,m]=size(B);

U=ab2u(A,B);

matU=[mat2vec(eye(n)) U];
vecr=eye(n^2-m^2-2*m*(n-m)+1,1)*n;

lambda0=eye(size(vecr));

[t,y]=ode45('dlam',[0 1],lambda0);

lambda=y(length(t),:);
v=matU*lambda';
V=vec2mat(v);
X=inv(V);

Pnew=Phlf*X*Phlf';