cir_cir_intersection.asv

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

function [ex,ey]=cir_cir_intersection(bx,by,d)

        
        if diff(by)/diff(bx) > 1e-2
            a_c=-(bx(1)-bx(2))/(by(1)-by(2)+eps);
            b_c=((bx(1)^2-bx(2)^2)+(by(1)^2-by(2)^2)-(d(1)^2-d(2)^2))/(2*(by(1)-by(2))+eps);
        
            c_c=1+a_c^2;
            d_c=-2*bx(1)+2*a_c*(b_c-by(1));
            e_c=bx(1)^2+(b_c-by(1))^2-d(1)^2;
        
            ex=[ (-d_c+sqrt(d_c^2-4*c_c*e_c))/(2*c_c)  (-d_c-sqrt(d_c^2-4*c_c*e_c))/(2*c_c)];
            ey=a_c*ex+b_c;
        else
            bc=-(by(1)-by(2))/(bx(1)-bx(2)+eps);
            ac=((bx(1)^2-bx(2)^2)+(by(1)^2-by(2)^2)-(d(1)^2-d(2)^2))/(2*(bx(1)-bx(2))+eps);
            
            a=1+bc^2;
            b=-(2*bx(1)*bc+2*by(1)-2*ac*bc);
            c=ac^2-2*bx(1)*ac
            
            
        end