cost_est.m

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

function [cost, theta]=cost_est(A,b,p,q)
%
%
%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        limda=limda_est(A,b,p,q);
        
        if limda(1) ~= -1
            theta=[-1 -1 -1]';
            cost=-1;            
            for m=1:length(limda)
                t_theta(:,m)=inv(A'*A+limda(m)*p)*(A'*b-0.5*limda(m)*q);
                if t_theta(3,m)>0
                    cost=(A*t_theta(:,m)-b)'*(A*t_theta(:,m)-b);
                    theta=t_theta(:,m);
                end
            end
        else
            theta=[-1 -1 -1]';
            cost=-1;
        end
         
   %%%%%%%%%%%%%%% limda is alsways zero %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 theta=inv(A'*A)*(A'*b);
%                 cost=(A*theta-b)'*(A*theta-b);