2、室内定位-基于RSSI测距的三维算法

>> a_x=[0 0 7 7];
>> b_y=[0 7 7 0];
>> c_z=[7 7 7 7]
>
>>  d=[10; 11; 12; 13]

>> Q=[-2*a_x(1) -2*b_y(1) -2*c_z(1) 1;
-2*a_x(2) -2*b_y(2) -2*c_z(2) 1;
-2*a_x(3) -2*b_y(3) -2*c_z(3) 1;
-2*a_x(4) -2*b_y(4) -2*c_z(4) 1]
>> temp =[a_x(1)^2 + b_y(1)^2 + c_z(1)^2; a_x(2)^2 + b_y(2)^2 + c_z(2)^2; a_x(3)^2 + b_y(3)^2 + c_z(3)^2; a_x(4)^2 + b_y(4)^2 + c_z(4)^2]

>>b = d - temp

get_xyzR2=(inv(Q'*Q))*Q'*b

beacon_x=[0 0 7 7];
beacon_y=[0 7 7 0];
beacon_z=[7 7 7 7];

Q=2*[beacon_x(4)- beacon_x(1) beacon_y(4)- beacon_y(1) beacon_z(4)- beacon_z(1);beacon_x(4)- beacon_x(2) beacon_y(4)- beacon_y(2) beacon_z(4)- beacon_z(2);beacon_x(4)- beacon_x(3) beacon_y(4)- beacon_y(3) beacon_z(4)- beacon_z(3)]

bieacon_d=[8;10;11;12];
>>  b=[bieacon_d(1)^2-bieacon_d(4)^2; bieacon_d(2)^2-bieacon_d(4)^2; bieacon_d(3)^2-bieacon_d(4)^2]

//inv(Q'*Q)可能不存在
>> get_xyz=(inv(Q'*Q))*Q'*b