前言
最近公司在研发室内定位的产品，作为后端工程师天然也开始了 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
@NoArgsConstructor
public 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
@NoArgsConstructor
public 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;
}