limda_est.asv

这是在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=1e3;
%             flag=0;
%             down=-1e7;
%             while flag==0
%                 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=[-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;