【MATLAB例程】基于RSSI原理的Wi-Fi定位程序，N个锚点（数量可自适应）、三维空间，轨迹使用CKF进行滤波，附代码下载链接-CSDN博客

本文链接：https://blog.csdn.net/callmeup/article/details/147530264

在这里插入图片描述

本文所述的MATLAB代码实现了一种基于接收信号强度指示（RSSI）的三维无线定位算法，结合容积卡尔曼滤波（CKF）对动态目标的运动轨迹进行优化。程序支持自适应锚点数量，适用于复杂环境下的高精度定位需求。

文章目录

程序介绍
参考文献与改进建议
运行结果
MATLAB源代码

程序介绍

功能模块与流程

环境初始化与基站生成
- 通过正弦/余弦函数生成三维空间中随机分布的锚点（基站），并添加微小偏移以模拟实际部署场景。
- 设置RSSI信号衰减模型参数（RSSI_0为参考距离信号强度，nn为路径损耗因子），模拟真实环境中的信号衰减特性。
目标运动与RSSI测量
- 生成目标的动态轨迹（三维正弦运动叠加线性位移），并基于RSSI模型计算各锚点与目标间的距离，加入高斯噪声模拟测量误差。
- 调用rssi_localization函数，通过加权最小二乘法解算初始位置估计，构建非线性观测方程。
CKF滤波优化
- 状态模型：假设目标运动为匀速模型（含过程噪声），状态向量为三维坐标。
- 容积点采样：通过Cholesky分解生成容积点，经非线性状态转移函数（含运动方程）传播，计算预测均值和协方差。
- 量测更新：利用观测值（RSSI定位结果）修正状态估计，通过卡尔曼增益调整权重，输出滤波后的轨迹。
可视化与误差分析
- 绘制三维轨迹图，对比真实值、观测值、CKF估计值及未滤波的惯导模拟结果。
- 计算各轴误差及均方根误差（RMSE），量化不同方法的定位精度。

算法核心与创新点

RSSI定位的改进
- 采用自适应锚点数量设计，通过伪逆法求解超定方程组，避免传统三边定位对锚点几何分布的强依赖。
- 引入路径损耗因子动态调整，增强模型对不同环境（如室内多径效应）的适应性。
CKF滤波的优势
- 对比扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF），CKF通过球面径向容积准则采样，以三阶精度逼近非线性积分，显著降低高维状态估计的误差。
- 代码中通过chol(P)分解协方差矩阵生成容积点，避免UKF参数调节的复杂性，提升数值稳定性。
多源数据融合
- 将RSSI观测值与运动模型结合，通过CKF实现传感器融合，有效抑制RSSI噪声和惯导累积误差，适用于复杂动态环境。

应用场景与扩展性

典型应用
- 室内导航：商场、医院等场景的人员/设备追踪。
- 工业物联网：仓储机器人定位、AGV轨迹优化。
- 应急救援：消防员在复杂建筑内的实时位置监控。
扩展方向
- 多模态融合：结合IMU或UWB数据，进一步提升鲁棒性。
- 自适应噪声调节：根据环境动态调整过程噪声协方差矩阵Q和观测噪声R。
- 实时性优化：通过并行计算或嵌入式部署，满足实时定位需求。

代码结构示例

% 主要模块
1. 初始化参数与生成基站坐标
2. 模拟目标运动并计算RSSI观测值
3. CKF滤波迭代（时间更新 + 量测更新）
4. 结果可视化与误差分析

% 核心函数
- rssi_localization()：RSSI定位解算
- position_3dim()：三维最小二乘估计
- CKF滤波循环：容积点生成、非线性映射、协方差更新

参考文献与改进建议

理论依据：CKF算法源于Arasaratnam的球面径向容积准则，在导航领域已验证其优于EKF/UKF的稳定性。
改进计算速度的方法：
- 增加抗差自适应机制，抑制异常观测值影响。
- 采用滑动窗口滤波，优化计算效率以支持实时应用。

运行结果

定位示意图：
三轴误差图：

在这里插入图片描述
命令行窗口的输出截图：

MATLAB源代码

部分代码：

% 基于RSSI原理的Wi-Fi定位程序，N个锚点（数量可自适应）、三维空间，轨迹使用CKF进行滤波
% 2025-04-26/Ver1

clear; clc; close all; % 清除工作区、命令窗口和关闭所有图形窗口
rng(0); %固定随机数种子
RSSI_err = 0.1; % 定义RSSI测量误差
n = 10; %定义蓝牙基站的数量
% 使用正弦和余弦函数生成各个蓝牙基站的坐标（并添加微小随机偏移）
baseP = 10*[sin(1:n)+0.01*[1:n]+1;cos(4*(1:n))+0.01*[1:n]+1;cos(2*(1:n))+0.01*[1:n]+1]';
% 定义信号强度与距离的关系
% 假设信号强度衰减模型为: RSSI(d) = RSSI_0 - 10*n*log10(d)
RSSI_0 = -30; % 在1米处的信号强度
nn = 5; % 衰减因子
% 模拟未知点的位置
% true_position = [10,10,20]; % 待定位点真实坐标

% 生成目标点坐标
position0 = [10,10,15];
% 生成目标的运动
positions = repmat(position0,21,1)+[0:0.2:4;0:-0.2:-4;zeros(1,21)]';

for i1 = 1:size(positions,1)
    true_position = positions(i1,:);

    distances(:,i1) = sqrt(sum((baseP - true_position).^2, 2)); % 计算距离
    RSSI_measurements(:,i1) = RSSI_0 - 10*nn*log10(distances(:,i1)) + RSSI_err*randn(size(distances(:,i1))); % 添加噪声
    % 定位函数
    estimated_position(i1,:) = rssi_localization(RSSI_measurements(:,i1), baseP, RSSI_0, nn);
end

estimated_position_wifi = estimated_position';

%% 滤波模型初始化
t = 1:1:size(positions,1);% 定义时间序列
Q = 0.01*diag([1,1,1]);% 设置过程噪声协方差矩阵
w = sqrt(Q)*randn(size(Q,1),length(t)); %生成
% 观测噪声协方差矩阵和观测噪声
R = 0.1*diag([1,1,1]);
% v = sqrt(R)*randn(size(R,1),length(t));