初级会员

- 积分
- 73
- 金钱
- 73
- 注册时间
- 2018-5-13
- 在线时间
- 32 小时
|

楼主 |
发表于 2019-9-17 21:33:58
|
显示全部楼层
clc;
clear all; %清除所有变量
%**************************************************************
%%数据读取和处理
Aacx=xlsread('zuheshiyan.xlsx','A1:A9349');Aacx=Aacx'/16384;
Aacy=xlsread('zuheshiyan.xlsx','B1:B9349');Aacy=Aacy'/16384;
gyroz=xlsread('zuheshiyan.xlsx','D1 9349');gyroz=gyroz'/16.4/57.3;
DuwbA=xlsread('zuheshiyan.xlsx','E1:E9349');DuwbA=DuwbA'/100;
DuwbB=xlsread('zuheshiyan.xlsx','F1:F9349');DuwbB=DuwbB'/100;
DuwbC=xlsread('zuheshiyan.xlsx','G1:G9349');DuwbC=DuwbC'/100;
%**************************************************************
%%UWB基站坐标
UwbA.x=0;UwbA.y=0;
UwbB.x=15;UwbB.y=0;
UwbC.x=0;UwbC.y=15;
%**************************************************************
%%UWB解算 最小二乘法
b=[];
A=2*[(UwbC.x-UwbA.x),(UwbC.y-UwbA.y);(UwbC.x-UwbB.x),(UwbC.y-UwbB.y)];
for i=1:9349
b=[DuwbA(i)^2-DuwbC(i)^2+UwbC.x^2+UwbC.y^2-UwbA.x^2-UwbA.y^2;DuwbB(i)^2-DuwbC(i)^2+UwbC.x^2+UwbC.y^2-UwbB.x^2-UwbB.y^2];
Estimate_UWB=inv(A'*A)*A'*b;
Est_UWB_target(i).x=Estimate_UWB(1);Est_UWB_target(i).y=Estimate_UWB(2);
%x(i)= Est_UWB_target(i).x;
%y(i)= Est_UWB_target(i).y;
end
figure
hold on;box on;
h1=plot(Est_UWB_target.x,Est_UWB_target.y); |
|