cost_est.asv

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

function [cost, theta]=cost_est(A,b,p)
%
%
%
        limda=limda_est(A,b,p)
        flag(1:length(limda))=0;
        if limda(1) ~= -1
            for m=1:length(limda)
                t_theta(:,m)=inv(A'*A+limda(m)*p)*A'*b;
                if t_theta(3,m)>0
                    flag(m)=1;
                end
            end
            
%             for m=1:length(limda)
%                 if t_theta(3,m)<0  % || abs(theta(1)^2+theta(2)^2-theta(3)^2)>50
%                     cost=-1;
%                     theta=[-1 -1 -1]';
%                 else
%                     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