前言
最近公司在研发室内定位的产品，作为后端工程师天然也开始了UWB定位算法的钻研。
协定
解析
依据协定内容，咱们能够简略编写工具类，疾速解析
private static byte[] getSHA1Digest(String data) throws IOException { byte[] bytes = null; try { MessageDigest md = MessageDigest.getInstance("SHA-1"); bytes = md.digest(data.getBytes("UTF-8")); } catch (GeneralSecurityException gse) { throw new IOException(gse); } return bytes; } /** * 二进制转十六进制字符串 * * @param bytes * @return */ private static String byte2hex(byte[] bytes) { StringBuilder sign = new StringBuilder(); for (int i = 0; i < bytes.length; i++) { String hex = Integer.toHexString(bytes[i] & 0xFF); if (hex.length() == 1) { sign.append("0"); } sign.append(hex.toUpperCase()); } return sign.toString(); } /** * 将16进制转换为二进制 * * @param hexStr * @return */ public static byte[] parseHexStr2Byte(String hexStr) { if (hexStr.length() < 1) { return null; } byte[] result = new byte[hexStr.length() / 2]; for (int i = 0; i < hexStr.length() / 2; i++) { int high = Integer.parseInt(hexStr.substring(i * 2, i * 2 + 1), 16); int low = Integer.parseInt(hexStr.substring(i * 2 + 1, i * 2 + 2), 16); System.out.println("字节位：" + (byte) (high * 16 + low)); result[i] = (byte) (high * 16 + low); } System.out.println(new String(result)); return result; } /** * 十六进制字符串转二进制字符串 * * @param hexStr 十六进制字符串 * @return 二进制字符串 */ public static String hexToBin(String hexStr) { String hexToBinStr = ""; try { hexToBinStr = Long.toBinaryString(Long.valueOf(hexStr, 16)); } catch (NumberFormatException e) { e.printStackTrace(); } return hexToBinStr; } /** * 十六进制字符串转十进制 * * @param hexStr 十六进制字符串 * @return 二进制字符串 */ public static int hexToTen(String hexStr) { BigInteger lngNum = new BigInteger(hexStr, 16); return lngNum.intValue(); } public static void main(String[] args) { String payload = "mc0f00000663000005a300000512000004cbffffffffffffffffffffffffffffffff095fc100146fb7a0:022be"; String hexStr = payload.substring(2, 4); String mask = hexToBin(hexStr); System.out.println("音讯头，固定为 mc:" + mask); String range = payload.substring(4, 68); System.out.println("基站A0-A8：" + range); System.out.println(range.length()); // 1:通过渠道获取imei // 2:通过imei获取该设施绑定的室内地图，通过地图找到地图x,y,z轴坐标 List<BaseStation> stationList = new ArrayList<>(); for (int i = 0; i < mask.length(); i++) { String result = mask.substring(mask.length() - 1 - i, mask.length() - i); if (result.equals("1")) { Double rawDistance = Double.valueOf(hexToTen(range.substring(i * 8, (i + 1) * 8))) / 1000; System.out.println("标签到A" + i + "的间隔:" + rawDistance); BaseStation baseStation = new BaseStation(); baseStation.setImei("imei"); baseStation.setRawDistance(rawDistance); stationList.add(baseStation); } } // baseStationService.calculate(stationList); }UWB定位算法编写
BaseStation.java
package com.bbzn.device.client.dataobject;import lombok.Getter;import lombok.NoArgsConstructor;import lombok.Setter;import lombok.ToString;import java.io.Serializable;import java.util.Date;@Getter@Setter@ToString@NoArgsConstructorpublic class BaseStation implements Serializable { private Long id; /** * 室内定位X轴 */ private Integer xAxis; /** * 室内定位Y轴 */ private Integer yAxis; /** * 室内定位Z轴 */ private Integer zAxis; /** * 设施imei号 */ private String imei; /** * 状态 0 默认 1 禁用 2 失常 */ private Integer status; /** * 创立工夫 */ private Date createTime; /** * 更新工夫 */ private Date updateTime; /** * 获取的间隔 */ private Double rawDistance; private static final long serialVersionUID = 1L;}计算定位坐标：
/** * 计算定位坐标 * * @param bases 接管到的一组基站对象列表(此处列表中的基站该当是id各异的) * @return 返回定位坐标 */ @Override public Location calculate(List<BaseStation> bases) { Location location = new Location(); int baseNum = bases.size(); /*间隔数组*/ double[] distanceArray = new double[baseNum]; String[] ids = new String[baseNum]; int j = 0; /*取得基站id*/ for (BaseStation baseHeight : bases) { ids[j] = baseHeight.getId().toString(); distanceArray[j] = UwbMathUtils.getDistance(baseHeight.getZAxis(),baseHeight.getRawDistance()); j++; } int disArrayLength = distanceArray.length; double[][] a = new double[baseNum - 1][2]; double[][] b = new double[baseNum - 1][1]; /*数组a初始化*/ for (int i = 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 (int i = 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(distanceArray[i], 2); } /*将数组封装成矩阵*/ Matrix b1 = new Matrix(b); Matrix a1 = new Matrix(a); /*求矩阵的转置*/ Matrix a2 = a1.transpose(); /*求矩阵a1与矩阵a1转置矩阵a2的乘积*/ Matrix tmpMatrix1 = a2.times(a1); Matrix reTmpMatrix1 = tmpMatrix1.inverse(); Matrix tmpMatrix2 = reTmpMatrix1.times(a2); /*两头后果乘以最初的b1矩阵*/ Matrix resultMatrix = tmpMatrix2.times(b1); double[][] resultArray = resultMatrix.getArray(); location.setX(resultArray[0][0]); location.setY(resultArray[1][0]); /*设置定位后果的工夫戳*/ Timestamp ts = new Timestamp(System.currentTimeMillis());// location.setTimeStamp(ts); return location; }Location.java
@Getter@Setter@ToString@NoArgsConstructorpublic class Location implements Serializable { private static final long serialVersionUID = 1L; /*x轴坐标*/// private Double xAxis; /*x轴坐标*/ private Double x; /*y轴坐标*/// private Double yAxis; /*y轴坐标*/ private Double y; /*工夫戳*/// private Timestamp timeStamp; private String imei;}