基于Wi-Fi的定位系统的设计(一)

系统介绍

本系统涵盖前端、后端、安卓。我将分几个章节大致的讲下系统实现以及相关的源码介绍。先介绍后端的关键部分。

组合算法

使用这个算法的原因是由于扫描到的定位路由锚点可能存在多种组合，通过不同的组合进行计算再通过一定的选择策略确定未知点坐标。

package com.jessevolka.rwp.util;


import java.util.ArrayList;
import java.util.List;

/**
 * <p></p>
 * <br/>
 *
 * @author fanmf
 * @date 2020-04-06 15:27:16
 */
public class Combination<T> {
    /**
     * 组合后的结果
     */
    private List<List<T>> result = new ArrayList<>();

    public List<List<T>> getResult() {
        return result;
    }

    /**
     * <p>
     * 组合算法
     * python改写java by fanmf
     * 假设我们要从上面的数组中选出3个元素出来。
     * 我们首先从第一个元素下手，对于第一个元素，我们有两个选择：要 or 不要。
     * 如果要了，那么我们需要选择的元素就少了一个了，我们只需要从后面的元素中选出两个就够了。
     * 如果不要，我们就从第二个元素继续看，此时我们还是要选出三个
     * 链接：https://www.jianshu.com/p/4bab880bbe69
     * <p>
     * <br/>
     *
     * @param data           需要组合的数据
     * @param step           第几步
     * @param selectedData   已选择的数据
     * @param selectQuantity 选择的数量
     * @author fanmf
     * @date 2020/4/6 15:32
     */
    public void combine(List<T> data, int step, List<T> selectedData, int selectQuantity) {
        if (selectedData.size() == selectQuantity) {
            List<T> item = new ArrayList<>(selectedData);
            result.add(item);
            return;
        }
        if (step >= data.size()) {
            return;
        }
        selectedData.add(data.get(step));
        combine(data, step + 1, selectedData, selectQuantity);
        selectedData.remove(selectedData.size() - 1);
        combine(data, step + 1, selectedData, selectQuantity);
    }

}

三点定位

相关代码解释看注释，不明白的地方可以提问。

package com.jessevolka.rwp.algorithm;

import com.jessevolka.rwp.common.exception.UnifiedException;

import static java.lang.Math.abs;
import static java.lang.Math.pow;

/**
 * <p></p>
 * <br/>
 *
 * @author fanmf
 * @date 2020-03-18 19:32:54
 */
public class PositionByThreePointAlg {

    /**
     * <p>矩阵相加<p>
     * <br/>
     *
     * @param matrixA 矩阵A
     * @param matrixB 矩阵B
     * @return double[][]
     * @author fanmf
     * @date 2020/3/18 19:38
     */
    private double[][] getMatrixAdd(double[][] matrixA, double[][] matrixB) {
        //判断矩阵是否可以相加 即行数与列数都相等
        if (matrixA.length != matrixB.length || matrixA[0].length != matrixB[0].length) {
            throw new UnifiedException("此矩阵不可以相加！");
        }
        for (int i = 0; i < matrixA.length; i++) {
            for (int j = 0; j < matrixA[0].length; j++) {
                matrixA[i][j] = matrixA[i][j] + matrixB[i][j];
            }
        }
        return matrixA;
    }

    /**
     * <p>矩阵相减<p>
     * <br/>
     *
     * @param matrixA 矩阵A
     * @param matrixB 矩阵B
     * @return double[][]
     * @author fanmf
     * @date 2020/3/18 19:39
     */
    private double[][] getMatrixSub(double[][] matrixA, double[][] matrixB) {
        if (matrixA.length != matrixB.length || matrixA[0].length != matrixB[0].length) {
            throw new UnifiedException("此矩阵不可以相减！");
        }
        for (int i = 0; i < matrixA.length; i++) {
            for (int j = 0; j < matrixA[0].length; j++) {
                matrixA[i][j] = matrixA[i][j] - matrixB[i][j];
            }
        }
        return matrixA;
    }

    /**
     * <p>矩阵相乘<p>
     * <br/>
     *
     * @param matrixA 矩阵A
     * @param matrixB 矩阵B
     * @return double[][]
     * @author fanmf
     * @date 2020/3/18 19:40
     */
    private double[][] getMatrixMul(double[][] matrixA, double[][] matrixB) {
        if (matrixA[0].length != matrixB.length) {
            throw new UnifiedException("此矩阵不可以相乘！");
        }
        double[][] matrixC = new double[matrixA.length][matrixB[0].length];
        for (int i = 0; i < matrixA.length; i++) {
            for (int j = 0; j < matrixB[0].length; j++) {
                matrixC[i][j] = 0;
                for (int m = 0; m < matrixA[0].length; m++) {
                    matrixC[i][j] = matrixC[i][j] + matrixA[i][m] * matrixB[m][j];
                }
            }
        }
        return matrixC;
    }

    /**
     * <p>全选主元高斯-约旦消元法 求逆矩阵<p>
     * <br/>
     *
     * @param matrix 方阵参数
     * @return double[][]
     * @author fanmf
     * @date 2020/3/18 19:37
     */
    private double[][] getInverseMatrixByGJ(double[][] matrix) {
        if (matrix.length != matrix[0].length) {
            throw new UnifiedException("此矩阵没有逆矩阵！");
        }
        double[][] result = new double[matrix.length][matrix[0].length];
        for (int i = 0; i < matrix.length; i++) {
            for (int j = 0; j < matrix.length; j++) {
                if (i == j) {
                    result[i][j] = 1;
                } else {
                    result[i][j] = 0;
                }
            }
        }

        for (int i = 0; i < matrix.length; i++) {
            double max = 0;
            int max_ind = 0;
            //寻找当前列的最大值
            for (int j = i; j < matrix.length; j++) {
                if (abs(matrix[i][j]) >= max) {
                    max_ind = j;
                    max = abs(matrix[i][j]);
                }
            }
            //把最大值移动到主对角线上
            if (max_ind != i) {
                double[] a = matrix[i];
                matrix[i] = matrix[max_ind];
                matrix[max_ind] = a;
                a = result[i];
                result[i] = result[max_ind];
                result[max_ind] = a;
            }
            //对角线上的元素称为主元
            double a = matrix[i][i];

            //主元为0则为奇异矩阵，无逆矩阵
            if (isEqual(a, 0)) {
                throw new UnifiedException("主元为0则为奇异矩阵，无逆矩阵!");
            }
            //将$arr主对角线上的元素化为1
            for (int j = i; j < matrix.length; j++) {
                matrix[i][j] = matrix[i][j] / a;
            }
            for (int j = 0; j < matrix.length; j++) {
                result[i][j] = result[i][j] / a;
            }
            //将当前列非主对角线上的元素变为0
            for (int j = 0; j < matrix.length; j++) {
                if (j != i) {
                    double c = matrix[j][i] / matrix[i][i];
                    for (int k = 0; k < matrix.length; k++) {
                        matrix[j][k] = matrix[j][k] - matrix[i][k] * c;
                        result[j][k] = result[j][k] - result[i][k] * c;
                    }
                }

            }
        }
        return result;
    }

    /**
     * <p>三点定位 公式=A*X=B X=A^-1*B<p>
     * <br/>
     *
     * @param matrix 无线路由坐标及距离 例如{{x1,y1,d1},{x2,y2,d2},{x3,y3,d3}}
     * @return double[][]
     * @author fanmf
     * @date 2020/3/18 19:40
     */
    public double[][] getPositionByThreePoint(double[][] matrix) {
        //求A矩阵
        double[][] matrixA = new double[2][2];
        matrixA[0][0] = 2 * (matrix[0][0] - matrix[2][0]);
        matrixA[0][1] = 2 * (matrix[0][1] - matrix[2][1]);
        matrixA[1][0] = 2 * (matrix[1][0] - matrix[2][0]);
        matrixA[1][1] = 2 * (matrix[1][1] - matrix[2][1]);
        //求B矩阵
        double[][] matrixB = new double[2][1];
        matrixB[0][0] = pow(matrix[2][2], 2) - pow(matrix[0][2], 2) + pow(matrix[0][0], 2) - pow(matrix[2][0], 2) + pow(matrix[0][1], 2) - pow(matrix[2][1], 2);
        matrixB[1][0] = pow(matrix[2][2], 2) - pow(matrix[1][2], 2) + pow(matrix[1][0], 2) - pow(matrix[2][0], 2) + pow(matrix[1][1], 2) - pow(matrix[2][1], 2);
        //求A^-1
        double[][] inverseMatrix = getInverseMatrixByGJ(matrixA);
        //求x,y
        return getMatrixMul(inverseMatrix, matrixB);
    }

    private boolean isEqual(double a, double b) {
        return abs(a - b) < 0.00000001;
    }

    private void printMatrix(double[][] params) {
        for (double[] param : params) {
            for (int j = 0; j < params[0].length; j++) {
                System.out.print(param[j] + " ");
            }
            System.out.println();
        }
    }
}

KNN定位

主要是分享下大致思路。

package com.jessevolka.rwp.algorithm;

import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
import com.jessevolka.rwp.common.enums.ExceptionEnum;
import com.jessevolka.rwp.common.exception.UnifiedException;
import com.jessevolka.rwp.model.*;
import com.jessevolka.rwp.model.dto.PositionQueryDTO;
import com.jessevolka.rwp.service.DeviceRssiStrengthService;
import com.jessevolka.rwp.service.WirelessDeviceCoordinatesService;
import org.springframework.stereotype.Component;

import javax.annotation.PostConstruct;
import javax.annotation.Resource;
import java.math.BigDecimal;
import java.util.*;
import java.util.stream.Collectors;

/**
 * <p>KNN定位算法</p>
 * <br/>
 *
 * @author fanmf
 * @since 2020-03-31 21:56:57
 */
@SuppressWarnings("unused")
@Component
public class PositionByKNNAlg {

    /**
     * 未知点测得的信号强度列表
     */
    private List<WiFiRSSI> params;

    @Resource
    private DeviceRssiStrengthService deviceRssiStrengthService;
    @Resource
    private WirelessDeviceCoordinatesService wirelessDeviceCoordinatesService;

    private static DeviceRssiStrengthService staticDeviceRssiStrengthService;

    private static WirelessDeviceCoordinatesService staticWirelessDeviceCoordinatesService;

    @PostConstruct
    public void init() {
        staticDeviceRssiStrengthService=deviceRssiStrengthService;
        staticWirelessDeviceCoordinatesService=wirelessDeviceCoordinatesService;
    }

    /**
     * <p><p>
     * <br/>
     *
     * @param positionQueryDTO 未知点测得的路由器信号强度列表及节点id
     * @return Coordinates 未知点坐标
     * @author fanmf
     * @since 2020/4/1 21:27
     */
    public Coordinates KNNPositioning(PositionQueryDTO positionQueryDTO) {
        List<KNNModel> knnModels = new ArrayList<>();
        Map<String, List<DeviceRssiStrength>> listMap = staticDeviceRssiStrengthService.list(new QueryWrapper<DeviceRssiStrength>().eq("flor_plan_dir_id", positionQueryDTO.getFlorPlanDirId()))
                .stream().collect(Collectors.groupingBy(element -> element.getXCoor() + "-" + element.getYCoor()));
        Set<String> deviceNames = staticWirelessDeviceCoordinatesService.list(new QueryWrapper<WirelessDeviceCoordinates>().eq("flor_plan_dir_id", positionQueryDTO.getFlorPlanDirId())).stream().map(WirelessDeviceCoordinates::getWireDeviName).collect(Collectors.toSet());
        this.params = positionQueryDTO.getWiFiRSSIS().stream().filter(element -> deviceNames.contains(element.getSsid())).collect(Collectors.toList());
        Set<String> scanssid = params.stream().map(WiFiRSSI::getSsid).collect(Collectors.toSet());
        //最低路由器数量
        int threshold = 3;
        if (params.size() >= threshold) {
            listMap.keySet().forEach(element -> {
                List<DeviceRssiStrength> deviceRssiStrengths = listMap.get(element).stream().filter(element2 -> scanssid.contains(element2.getWireDeviName())).collect(Collectors.toList());
                if (deviceRssiStrengths.size() == params.size()) {
                    knnModels.add(calculate(deviceRssiStrengths));
                }
            });
            Collections.sort(knnModels);
            return getUnknownPointCoordinates(knnModels);
        }
        throw new UnifiedException(ExceptionEnum.LESS_THAN_THREE_ANCHOR);
    }

    /**
     * <p><p>
     * <br/>
     *
     * @param knnModels 每个坐标到未知点的“距离”
     * @return com.jessevolka.rwp.model.Coordinates
     * @author fanmf
     * @date 2020/4/4 18:38
     */
    private Coordinates getUnknownPointCoordinates(List<KNNModel> knnModels) {
        //K值 经验取值
        int k = 3;
        if (knnModels.size()<k){
            throw new UnifiedException(ExceptionEnum.DATA_TOO_SMALL);
        }
        double sum = 0;
        Coordinates coordinates = new Coordinates("0","0");
        for (int i = 0; i < k; i++) {
            KNNModel knnModel = knnModels.get(i);
            sum += new BigDecimal(knnModel.getDistance()).doubleValue();
        }
        for (int i = 0; i < k; i++) {
            KNNModel knnModel = knnModels.get(i);
            double rate = (1 - (new BigDecimal(knnModel.getDistance()).doubleValue()) / sum);
            double increaseX = new BigDecimal(knnModel.getXCoor()).doubleValue() * rate;
            double increaseY = new BigDecimal(knnModel.getYCoor()).doubleValue() * rate;
            coordinates.setXCoor(new BigDecimal(coordinates.getXCoor()).doubleValue() + increaseX + "");
            coordinates.setYCoor(new BigDecimal(coordinates.getYCoor()).doubleValue() + increaseY + "");
        }
        return coordinates;
    }

    /**
     * <p><p>
     * <br/>
     *
     * @param strengths 数据库里已存在某坐标的训练数据
     * @return com.jessevolka.rwp.model.KNNModel
     * @author fanmf
     * @since 2020/4/1 21:49
     */
    private KNNModel calculate(List<DeviceRssiStrength> strengths) {
        KNNModel knnModel = new KNNModel();
        knnModel.setXCoor(strengths.get(0).getXCoor());
        knnModel.setYCoor(strengths.get(0).getYCoor());
        knnModel.setDistance("0");
        strengths.forEach(element -> params.forEach(element2 -> {
            if (element.getWireDeviName().equals(element2.getSsid())) {
                BigDecimal a = new BigDecimal(element.getRssiStrgth());
                BigDecimal b = new BigDecimal(element2.getLevel());
                double square = Math.pow(a.subtract(b).doubleValue(), 2);
                knnModel.setDistance(new BigDecimal(knnModel.getDistance()).doubleValue() + square + "");
            }
        }));
        return knnModel;
    }
}