limda_est.m

这是在uwb系统中,用TOA定位法的整个系统的仿真程序

function limda=limda_est(A,b,p,q)
%
%
%
 
%         M=1e6; cn=0;
%         for t=M:1:5*M
%             cn=cn+1;
%               f(cn)=q'*inv(A'*A+t*p)*(A'*b-0.5*t*q)+(inv(A'*A+t*p)*(A'*b-0.5*t*q))'*p*inv(A'*A+t*p)*(A'*b-0.5*t*q);
%         end
%         t=M:1:5*M;

        
        
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%             M=1e7;
%           inter=[0 M];
%           err=inf;
%           au=[];bu=[];
%           au=inter(1);bu=inter(2);
%           num=0;
%           while err> 1e-4 && num<1e4
%               num=num+1;
%               fau=q'*inv(A'*A+au*p)*(A'*b-0.5*au*q)+(inv(A'*A+au*p)*(A'*b-0.5*au*q))'*p*inv(A'*A+au*p)*(A'*b-0.5*au*q);
%               fbu=q'*inv(A'*A+bu*p)*(A'*b-0.5*bu*q)+(inv(A'*A+bu*p)*(A'*b-0.5*bu*q))'*p*inv(A'*A+bu*p)*(A'*b-0.5*bu*q);
% 
%               if fau*fbu<0
%                   cu=(au+bu)/2;
%                   fcu=q'*inv(A'*A+cu*p)*(A'*b-0.5*cu*q)+(inv(A'*A+cu*p)*(A'*b-0.5*cu*q))'*p*inv(A'*A+cu*p)*(A'*b-0.5*cu*q);
%                   if fcu<0
%                       au=cu;
%                   else
%                       bu=cu;
%                   end
%               end
%               err=abs(au-bu);
%           end
          

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            step=5*1e3;
            flag=0;
            down=-1e10; nf=0;
            while flag==0
                nf=nf+1
                fdown=q'*inv(A'*A+down*p)*(A'*b-0.5*down*q)+(inv(A'*A+down*p)*(A'*b-0.5*down*q))'*p*inv(A'*A+down*p)*(A'*b-0.5*down*q);
                up=down+step;
                fup=q'*inv(A'*A+up*p)*(A'*b-0.5*up*q)+(inv(A'*A+up*p)*(A'*b-0.5*up*q))'*p*inv(A'*A+up*p)*(A'*b-0.5*up*q);

                if fdown*fup<0
                    flag=1;
                end
                if fdown*fup>0
                    down=down+step;
                    up=down+step;
                end                
            end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
          inter=[down up];
%           inter=[-M 0];
          err=inf;
          au=[];bu=[];
          au=inter(1);bu=inter(2);
          num=0;
          while err> 1e-4 && num<1e4
              num=num+1;
              
              fau=q'*inv(A'*A+au*p)*(A'*b-0.5*au*q)+(inv(A'*A+au*p)*(A'*b-0.5*au*q))'*p*inv(A'*A+au*p)*(A'*b-0.5*au*q);
              fbu=q'*inv(A'*A+bu*p)*(A'*b-0.5*bu*q)+(inv(A'*A+bu*p)*(A'*b-0.5*bu*q))'*p*inv(A'*A+bu*p)*(A'*b-0.5*bu*q);
              if fau*fbu<0
                  cu=(au+bu)/2;
                  fcu=q'*inv(A'*A+cu*p)*(A'*b-0.5*cu*q)+(inv(A'*A+cu*p)*(A'*b-0.5*cu*q))'*p*inv(A'*A+cu*p)*(A'*b-0.5*cu*q);
                  if fcu<0
                      bu=cu;
                  else
                      au=cu;
                  end
              end
              err=abs(au-bu);
          end
          limda=cu;        
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            6;