前言公司近期在开发室内定位类产品。作为后端工程师，自然而然会开始研究UWB定位算法。协议解析根据协议内容，我们可以简单的编写工具类来快速解析privatestaticbyte[]getSHA1Digest(Stringdata)throwsIOException{byte[]bytes=null;尝试{MessageDigestmd=MessageDigest.getInstance("SHA-1");bytes=md.digest(data.getBytes("UTF-8"));}catch(GeneralSecurityExceptiongse){thrownewIOException(gse);}返回字节；}/***二进制到十六进制字符串**@parambytes*@return*/privatestaticStringbyte2hex(byte[]bytes){StringBuildersign=newStringBuilder();for(inti=0;istationList=newArrayList<>();对于(inti=0;ibases){Locationlocation=newLocation();intbaseNum=bases.size();/*距离数组*/double[]distanceArray=newdouble[baseNum];String[]ids=newString[baseNum];整数j=0；/*获取基站ID*/for(BaseStationbaseHeight:bases){ids[j]=baseHeight.getId().toString();distanceArray[j]=UwbMathUtils.getDistance(baseHeight.getZAxis(),baseHeight.getRawDistance());j++;}intdisArrayLength=distanceArray.length;double[][]a=newdouble[baseNum-1][2];double[][]b=newdouble[baseNum-1][1];/*数组a初始化*/for(inti=0;i<2;i++){a[i][0]=2*(bases.get(i).getXAxis()-bases.get(baseNum-1).getXAxis());a[i][1]=2*(bases.get(i).getYAxis()-bases.get(baseNum-1).getYAxis());}/*数组b初化*/for(inti=0;i<2;i++){b[i][0]=Math.pow(bases.get(i).getXAxis(),2)-Math.pow(bases.get(baseNum-1).getXAxis(),2)+Math.pow(bases.get(i).getYAxis(),2)-Math.pow(bases.get(baseNum-1).getYAxis(),2)+Math.pow(distanceArray[disArrayLength-1],2)-Math.pow(distanceAr光线[i],2);}/*将数组打包成矩阵*/Matrixb1=newMatrix(b);矩阵a1=新矩阵(a);/*找到矩阵的转置*/Matrixa2=a1.转置（）；/*求矩阵a1和矩阵a1的乘积转置矩阵a2*/MatrixtmpMatrix1=a2.times(a1);矩阵reTmpMatrix1=tmpMatrix1.inverse();矩阵tmpMatrix2=reTmpMatrix1.times(a2);/*中间结果乘以最终的b1矩阵*/MatrixresultMatrix=tmpMatrix2.times(b1);双[][]resultArray=resultMatrix.getArray();location.setX(resultArray[0][0]);location.setY(resultArray[1][0]);/*设置定位结果的时间戳*/Timestampts=newTimestamp(System.currentTimeMillis());//location.setTimeStamp(ts);返回位置；}Location.java@Getter@Setter@ToString@NoArgsConstructorpublicclassLocationimplementsSerializable{privatestaticfinallongserialVersionUID=1L;/*x轴坐标*///privateDoublexAxis;/*x轴坐标*/privateDoublex;/*y轴坐标*///privateDoubley轴；/*y轴坐标*/privateDoubley;/*时间戳*///私有时间戳timeStamp;私人字符串串号；}
