forked from ZhanLi/UWBIns
356 lines
15 KiB
Mathematica
356 lines
15 KiB
Mathematica
|
clc
|
|||
|
|
clear
|
|||
|
|
close all
|
|||
|
|
|
|||
|
|
addpath './lib';
|
|||
|
|
addpath './lib/gnss';
|
|||
|
|
addpath './lib/calbiration';
|
|||
|
|
addpath './lib/plot';
|
|||
|
|
addpath './lib/rotation';
|
|||
|
|
addpath './lib/geo';
|
|||
|
|
savepath
|
|||
|
|
|
|||
|
|
%% 说明
|
|||
|
|
% UWB IMU 融合算法，采用误差卡尔曼15维经典模型，伪距组合
|
|||
|
|
% PR(TOF) 伪距： UWB硬件给出的原始测量距离值
|
|||
|
|
% IMU: 加速度(3) 3轴陀螺(3) 共6维,,
|
|||
|
|
% noimal_state: 名义状态: 导航方程状态: 位置(3) 速度(3) 四元数(4) 共10维
|
|||
|
|
% err_state: KF误差状态: 位置误差(3) 速度误差(3) 失准角(3) 加速度计零偏(3) 陀螺零偏(3) 共15维
|
|||
|
|
% du: 零偏反馈: 加速度计零偏(3) 陀螺零偏(3)， 共6维
|
|||
|
|
|
|||
|
|
% 单位说明:
|
|||
|
|
% 加速度,加速度零偏: m/s^(2)
|
|||
|
|
% 角速度, 角速度(陀螺)零偏: rad/s
|
|||
|
|
% 角度 rad
|
|||
|
|
% 速度: m/s
|
|||
|
|
|
|||
|
|
%% 读取数据集
|
|||
|
|
load datas2
|
|||
|
|
dataset = datas;
|
|||
|
|
% dataset的数据结构如下
|
|||
|
|
% dataset
|
|||
|
|
% ├── imu
|
|||
|
|
% | ├── acc 加速度计三维数据
|
|||
|
|
% | ├── gyr 陀螺仪三维数据
|
|||
|
|
% | └── time 时间戳数组
|
|||
|
|
% ├── uwb
|
|||
|
|
% | ├── time 时间戳数组 1*17200
|
|||
|
|
% | ├── anchor 四个标签的坐标矩阵，x，y，z
|
|||
|
|
% | ├── tof 飞行时间，4*17200，表示4个基站在连续时间点上对同一移动标签的测距时间
|
|||
|
|
% | └── cnt 基站个数
|
|||
|
|
% └── pos
|
|||
|
|
|
|||
|
|
N = length(dataset.imu.time); % 时间序列的长度
|
|||
|
|
dt = mean(diff(dataset.imu.time)); % dt：IMU数据的平均采样周期。通过diff方法计算相邻时间戳的差值，并求平均值得到dt
|
|||
|
|
|
|||
|
|
% 故意删除一些基站及数据，看看算法在基站数量很少的时候能否有什么奇迹。。
|
|||
|
|
% dataset.uwb.anchor(:,1) = [];
|
|||
|
|
% dataset.uwb.tof(1,:) = [];
|
|||
|
|
|
|||
|
|
|
|||
|
|
% EKF融合使用的基站个数，融合算法最少2个基站就可以2D定位
|
|||
|
|
%dataset.uwb.cnt = size(dataset.uwb.anchor, 2);
|
|||
|
|
|
|||
|
|
|
|||
|
|
m_div_cntr = 0; % 量测分频器：计数器，用于记录已累计的IMU更新次数。当计数器达到 m_div 时，触发UWB量测更新，并重置计数器。
|
|||
|
|
m_div = 1; % 量测分频系数，每m_div次量测，才更新一次EKF量测(UWB更新), 可以节约计算量 或者 做实验看效果
|
|||
|
|
% UWB的采样率通常远低于IMU（例如IMU 100Hz，UWB 10Hz），直接每次融合会导致冗余计算。通过分频系数可以匹配两者的实际更新频率，避免不必要的计算。
|
|||
|
|
UWB_LS_MODE = 3; % 2 纯UWB解算采用2D模式， 3：纯UWB解算采用3D模式
|
|||
|
|
UWB_EKF_UPDATE_MODE = 3; % EKF 融合采用2D模式， 3: EKF融合采用3D模式
|
|||
|
|
|
|||
|
|
%% 数据初始化
|
|||
|
|
out_data.uwb = []; % 初始化输出数据结构
|
|||
|
|
out_data.uwb.time = dataset.uwb.time; % 存储UWB时间戳
|
|||
|
|
out_data.imu.time = dataset.imu.time; % 存储IMU时间戳
|
|||
|
|
out_data.uwb.anchor = dataset.uwb.anchor; % 存储基站坐标
|
|||
|
|
% pr = 0;
|
|||
|
|
% last_pr = 0;
|
|||
|
|
|
|||
|
|
%% 滤波参数初始化
|
|||
|
|
settings = uwb_imu_example_settings(); % 获取滤波器设置参数，使用uwb_imu_example_settings()函数获取滤波器的配置参数
|
|||
|
|
% 返回值是一个结构体settings，包含滤波器的各种参数
|
|||
|
|
R = diag(ones(dataset.uwb.cnt, 1)*settings.sigma_uwb^(2)); % 量测噪声协方差矩阵
|
|||
|
|
noimal_state = init_navigation_state(settings) ; % 初始化名义状态，10维：位置：（x,y,z）;速度（vx,vy,vz）;姿态四元数（q0,q1,q2,q3）
|
|||
|
|
% 名义状态：系统在理想条件下的理论状态，忽略噪声、扰动和模型误差
|
|||
|
|
err_state = zeros(15, 1); % 初始化误差状态,15维：位置误差（3维）；速度误差（3维）；姿态误差（3维）；加速度计零偏（3维）；陀螺仪零偏（3维）
|
|||
|
|
% 误差状态：实际状态与名义状态之间的偏差，由噪声、外部干扰或模型不确定性导致
|
|||
|
|
|
|||
|
|
%使用第一帧伪距作为初始状态
|
|||
|
|
pr = dataset.uwb.tof(:, 1); % 获取第一帧UWB测量数据
|
|||
|
|
% 初始位置解算
|
|||
|
|
% 测量值=真实值+零偏+噪声
|
|||
|
|
% 零偏是传感器在无输入信号时输出的非零偏移量，反应其基准点的系统性偏差
|
|||
|
|
% 伪距是UWB（超宽带）等无线定位系统中，通过信号传播时间计算出的等效距离测量值
|
|||
|
|
% noimal_state前三项:位置坐标，使用最小二乘法解算
|
|||
|
|
noimal_state(1:3) = multilateration(dataset.uwb.anchor, [ 1 1 0.99]', pr', UWB_LS_MODE);
|
|||
|
|
|
|||
|
|
du = zeros(6, 1); % 初始化零偏反馈
|
|||
|
|
[P, Q1, Q2, ~, ~] = init_filter(settings); % 初始化滤波器参数，此处初始化时使用~占位符，因为这个地方这两个参数没有意义，这样占位可以不占用内存
|
|||
|
|
|
|||
|
|
% 输出提示
|
|||
|
|
fprintf("共%d帧数据, IMU采样频率:%d Hz 共运行时间 %d s\n", N, 1 / dt, N * dt);
|
|||
|
|
fprintf("UWB基站个数:%d\n", dataset.uwb.cnt);
|
|||
|
|
fprintf("UWB量测更新频率为:%d Hz\n", (1 / dt) / m_div);
|
|||
|
|
fprintf("UWB EKF量测更新模式: %dD模式\n", UWB_EKF_UPDATE_MODE);
|
|||
|
|
fprintf("纯UWB最小二乘解算: %dD模式\n", UWB_LS_MODE);
|
|||
|
|
fprintf("EKF 滤波参数:\n");
|
|||
|
|
settings
|
|||
|
|
fprintf("开始滤波...\n");
|
|||
|
|
|
|||
|
|
|
|||
|
|
for k=1:N
|
|||
|
|
|
|||
|
|
acc = dataset.imu.acc(:,k);
|
|||
|
|
gyr = dataset.imu.gyr(:,k);
|
|||
|
|
|
|||
|
|
% 反馈 加速度bias, 陀螺bias
|
|||
|
|
% 测量值 = 真实值 + 零偏 + 噪声
|
|||
|
|
acc = acc - du(1:3); % du是零偏，（1：3）和（4：6）分别代表加速度和陀螺仪的零偏
|
|||
|
|
gyr = gyr - du(4:6); % 陀螺仪
|
|||
|
|
|
|||
|
|
% 捷联惯导解算
|
|||
|
|
p = noimal_state(1:3); % 位置坐标 [x; y; z]（单位：米）
|
|||
|
|
v = noimal_state(4:6); % 速度 [vx; vy; vz]（单位：m/s）
|
|||
|
|
q = noimal_state(7:10); % 姿态四元数 [q0; q1; q2; q3]
|
|||
|
|
% 捷联惯导解算：输入当前状态和IMU测量值，输出更新后的位置、速度和姿态
|
|||
|
|
% pvq是当前的状态，acc、gyr是加速度和陀螺仪的测量值，dt为时间步长，后面为重力向量
|
|||
|
|
[p, v, q] = nav_equ_local_tan(p, v, q, acc, gyr, dt, [0, 0, -9.8]'); % 东北天坐标系，重力简化为-9.8
|
|||
|
|
|
|||
|
|
% 小车假设：基本做平面运动，N系下Z轴速度基本为0，直接给0
|
|||
|
|
v(3) = 0;
|
|||
|
|
|
|||
|
|
% 更新数值
|
|||
|
|
noimal_state(1:3) = p;
|
|||
|
|
noimal_state(4:6) = v;
|
|||
|
|
noimal_state(7:10) = q;
|
|||
|
|
out_data.eul(k,:) = q2eul(q); % 四元数转欧拉角并存入数据表
|
|||
|
|
|
|||
|
|
% 生成F阵 G阵
|
|||
|
|
% 看函数注释，根据当前的状态、加速度测量值 acc 和采样时间 dt，生成状态转移矩阵 F 和过程噪声耦合矩阵 G
|
|||
|
|
[F, G] = state_space_model(noimal_state, acc, dt);
|
|||
|
|
|
|||
|
|
% 卡尔曼P阵预测公式
|
|||
|
|
P = F*P*F' + G*blkdiag(Q1, Q2)*G';
|
|||
|
|
|
|||
|
|
% log数据
|
|||
|
|
out_data.x(k,:) = noimal_state;
|
|||
|
|
out_data.delta_u(k,:) = du'; % 位置方差
|
|||
|
|
out_data.diag_P(k,:) = trace(P); % 姿态四元数方差
|
|||
|
|
|
|||
|
|
|
|||
|
|
%% EKF UWB量测更新
|
|||
|
|
m_div_cntr = m_div_cntr+1; %量测分频器
|
|||
|
|
% 每m_div次量测，才更新一次EKF量测(UWB更新)，此时m_div设置为1
|
|||
|
|
if m_div_cntr == m_div
|
|||
|
|
m_div_cntr = 0;
|
|||
|
|
|
|||
|
|
pr = dataset.uwb.tof(1:dataset.uwb.cnt, k); % pr是当前时刻（第k帧）所有UWB基站的TOF
|
|||
|
|
|
|||
|
|
%判断两次PR 差，如果差太大，则认为这个基站PR比较烂，不要了。相当于GNSS里的挑星
|
|||
|
|
% arr = find(abs(pr - last_pr) < 0.05);
|
|||
|
|
% last_pr = pr;
|
|||
|
|
% out_data.good_anchor_cnt(k,:) = length(arr); %记录挑出来的基站数
|
|||
|
|
%
|
|||
|
|
% if(isempty(arr))
|
|||
|
|
% continue;
|
|||
|
|
% end
|
|||
|
|
%
|
|||
|
|
% %构建 剔除不好的基站之后的基站位置矩阵和R矩阵
|
|||
|
|
% pr = pr(arr);
|
|||
|
|
% anch = dataset.uwb.anchor(:, arr);
|
|||
|
|
% R1 = R(arr, arr);
|
|||
|
|
|
|||
|
|
% 算了不挑基站了，所有基站直接参与定位，其实也差不太多
|
|||
|
|
anch = dataset.uwb.anchor;
|
|||
|
|
R1 = R; % 测量噪声协方差矩阵
|
|||
|
|
|
|||
|
|
% 量测方程
|
|||
|
|
% Y是根据当前状态 noimal_state 预测出来的各个基站到目标的距离
|
|||
|
|
% H是观测矩阵，表示距离对状态的偏导
|
|||
|
|
[Y, H] = uwb_hx(noimal_state, anch, UWB_EKF_UPDATE_MODE);
|
|||
|
|
|
|||
|
|
% 卡尔曼公式，计算K
|
|||
|
|
S = H*P*H'+R1; % system uncertainty 系统不确定度，用于衡量测量是否可信
|
|||
|
|
residual = pr - Y; % residual 或者叫新息 测量与预测的差值
|
|||
|
|
|
|||
|
|
%% 根据量测置信度给R一些增益 Self-Calibrating Multi-Sensor Fusion with Probabilistic
|
|||
|
|
%Measurement Validation for Seamless Sensor Switching on a UAV, 计算量测可靠性
|
|||
|
|
% 衡量量测可信度，L越大代表当前量测越不可信（Mahalanobis 距离L）
|
|||
|
|
L = (residual'*S^(-1)*residual);
|
|||
|
|
out_data.L(k,:) = L;
|
|||
|
|
|
|||
|
|
% if(L > 20 ) %如果量测置信度比较大，则更不相信量测
|
|||
|
|
% S = H*P*H'+R1*5;
|
|||
|
|
% end
|
|||
|
|
|
|||
|
|
K = (P*H')/(S); % 卡尔曼增益K
|
|||
|
|
err_state = [zeros(9,1); du] + K*(residual); % 状态误差向量
|
|||
|
|
|
|||
|
|
% 反馈速度位置
|
|||
|
|
noimal_state(1:6) = noimal_state(1:6) + err_state(1:6);
|
|||
|
|
|
|||
|
|
% 反馈姿态
|
|||
|
|
q = noimal_state(7:10); % 提取姿态四元数
|
|||
|
|
q = qmul(rv2q(err_state(7:9)), q);
|
|||
|
|
noimal_state(7:10) = q; % 姿态反馈（使用小角度修正旋转）
|
|||
|
|
|
|||
|
|
%存储加速度计零偏，陀螺零偏
|
|||
|
|
du = err_state(10:15);
|
|||
|
|
|
|||
|
|
% P阵后验更新
|
|||
|
|
P = (eye(15)-K*H)*P;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
%% 车载约束：Z轴速度约束： B系下 Z轴速度必须为0(不能钻地).. 可以有效防止Z轴位置跳动 参考https://kth.instructure.com/files/677996/download?download_frd=1 和 https://academic.csuohio.edu/simond/pubs/IETKalman.pdf
|
|||
|
|
%% 车辆在 B系Z轴上不应该有速度，如果估计有，就认为估计错了，通过观测去修正
|
|||
|
|
R2 = eye(1)*0.5;
|
|||
|
|
Cn2b = q2m(qconj(noimal_state(7:10))); % 计算四元数的共轭，得到从导航系到机体系的旋转;再转换成方向余弦矩阵
|
|||
|
|
|
|||
|
|
H = [zeros(1,3), [0 0 1]* Cn2b, zeros(1,9)]; % 构造观测矩阵H，用于观测当前状态中的速度
|
|||
|
|
|
|||
|
|
K = (P*H')/(H*P*H'+R2);
|
|||
|
|
z = Cn2b*noimal_state(4:6); % 当前估计的B系Z轴速度
|
|||
|
|
|
|||
|
|
err_state = [zeros(9,1); du] + K*(0-z(3:3)); % 目标观测值是0（因为车不能在B系Z轴上移动），当前估计值是z(3)，差值为残差
|
|||
|
|
|
|||
|
|
% 反馈速度位置 更新状态
|
|||
|
|
noimal_state(1:6) = noimal_state(1:6) + err_state(1:6);
|
|||
|
|
|
|||
|
|
% 反馈姿态
|
|||
|
|
q = noimal_state(7:10);
|
|||
|
|
q = qmul(ch_rv2q(err_state(7:9)), q); % 更新原始四元数
|
|||
|
|
noimal_state(7:10) = q;
|
|||
|
|
|
|||
|
|
%存储加速度计零偏，陀螺零偏
|
|||
|
|
% du = err_state(10:15);
|
|||
|
|
|
|||
|
|
% P阵后验更新
|
|||
|
|
P = (eye(15)-K*H)*P;
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
fprintf("开始纯UWB最小二乘位置解算...\n");
|
|||
|
|
%% 纯 UWB 位置解算
|
|||
|
|
j = 1;
|
|||
|
|
uwb_pos = [1 1 1]';
|
|||
|
|
N = length(dataset.uwb.time);
|
|||
|
|
|
|||
|
|
for i=1:N
|
|||
|
|
pr = dataset.uwb.tof(:, i);
|
|||
|
|
% 去除NaN点
|
|||
|
|
%if all(~isnan(pr)) == true
|
|||
|
|
|
|||
|
|
uwb_pos = multilateration(dataset.uwb.anchor, uwb_pos, pr', UWB_LS_MODE);
|
|||
|
|
out_data.uwb.pos(:,j) = uwb_pos;
|
|||
|
|
j = j+1;
|
|||
|
|
%end
|
|||
|
|
end
|
|||
|
|
fprintf("计算完成...\n");
|
|||
|
|
|
|||
|
|
%% plot 数据
|
|||
|
|
out_data.uwb.tof = dataset.uwb.tof;
|
|||
|
|
out_data.uwb.fusion_pos = out_data.x(:,1:3)';
|
|||
|
|
demo_plot(dataset, out_data);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
%% 初始化nomial state
|
|||
|
|
function x = init_navigation_state(~)
|
|||
|
|
|
|||
|
|
% 简单点， 全部给 0 。10维：位置：（x,y,z）;速度（vx,vy,vz）;姿态四元数（q0,q1,q2,q3）
|
|||
|
|
q = eul2q(deg2rad([0 0 0])); % eul2q函数用于将欧拉角转换为四元数
|
|||
|
|
x = [zeros(6,1); q]; % 拼接6维零向量与四元数
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
%% 初始化滤波器参数
|
|||
|
|
function [P, Q1, Q2, R, H] = init_filter(settings)
|
|||
|
|
|
|||
|
|
% Kalman filter state matrix
|
|||
|
|
P = zeros(15);
|
|||
|
|
P(1:3,1:3) = settings.factp(1)^2*eye(3);
|
|||
|
|
P(4:6,4:6) = settings.factp(2)^2*eye(3);
|
|||
|
|
P(7:9,7:9) = diag(settings.factp(3:5)).^2;
|
|||
|
|
P(10:12,10:12) = settings.factp(6)^2*eye(3);
|
|||
|
|
P(13:15,13:15) = settings.factp(7)^2*eye(3);
|
|||
|
|
|
|||
|
|
% Process noise covariance
|
|||
|
|
Q1 = zeros(6);
|
|||
|
|
Q1(1:3,1:3) = diag(settings.sigma_acc).^2*eye(3);
|
|||
|
|
Q1(4:6,4:6) = diag(settings.sigma_gyro).^2*eye(3);
|
|||
|
|
|
|||
|
|
Q2 = zeros(6);
|
|||
|
|
Q2(1:3,1:3) = settings.sigma_acc_bias^2*eye(3);
|
|||
|
|
Q2(4:6,4:6) = settings.sigma_gyro_bias^2*eye(3);
|
|||
|
|
|
|||
|
|
R =0;
|
|||
|
|
H = 0;
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
%% 生成F阵，G阵
|
|||
|
|
% 根据当前的状态 x、加速度测量值 acc 和采样时间 dt，生成状态转移矩阵 F 和过程噪声耦合矩阵 G
|
|||
|
|
function [F,G] = state_space_model(x, acc, dt)
|
|||
|
|
Cb2n = q2m(x(7:10)); % 把四元数转换成方向余弦矩阵，从机体坐标系（body）转到导航系（nav）
|
|||
|
|
|
|||
|
|
sk = askew(Cb2n * acc); % 对一个向量v构造出反对称矩阵，用于表示向量叉乘的线性化形式
|
|||
|
|
|
|||
|
|
O = zeros(3);
|
|||
|
|
I = eye(3);
|
|||
|
|
% P V theta 加计零偏 陀螺零偏
|
|||
|
|
F = [
|
|||
|
|
O I O O O;
|
|||
|
|
O O -sk -Cb2n O;
|
|||
|
|
O O O O -Cb2n;
|
|||
|
|
O O O O O;
|
|||
|
|
O O O O O];
|
|||
|
|
|
|||
|
|
% Approximation of the discret time state transition matrix
|
|||
|
|
% 将连续时间的状态转移矩阵做一次一阶近似离散化
|
|||
|
|
F = eye(15) + dt*F;
|
|||
|
|
|
|||
|
|
% Noise gain matrix
|
|||
|
|
G=dt*[
|
|||
|
|
O O O O;
|
|||
|
|
Cb2n O O O;
|
|||
|
|
O -Cb2n O O;
|
|||
|
|
O O I O;
|
|||
|
|
O O O I];
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
%% UWB量测过程
|
|||
|
|
% Y 根据当前状态和UWB基站坐标预测出来的伪距
|
|||
|
|
% H 量测矩阵
|
|||
|
|
% anchor: 基站坐标 M x N: M:3(三维坐标)， N:基站个数
|
|||
|
|
% dim: 2: 二维 3：三维
|
|||
|
|
function [Y, H] = uwb_hx(x, anchor, dim)
|
|||
|
|
N = size(anchor,2); % 基站个数
|
|||
|
|
|
|||
|
|
% 三维取xyz，二维取xy
|
|||
|
|
position = x(1:3);
|
|||
|
|
if(dim)== 2
|
|||
|
|
position = position(1:2);
|
|||
|
|
anchor = anchor(1:2, 1:N);
|
|||
|
|
% uwb.anchor
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
Y = []; % Y是伪距预测，即目标位置到每个锚点的欧几里得距离
|
|||
|
|
H = []; % 雅可比矩阵的每一行
|
|||
|
|
% 计算预测的伪距s，所有预测的位置与各个锚点的向量差
|
|||
|
|
perd_pr = repmat(position,1,N) - anchor(:,1:N);
|
|||
|
|
for i=1:N
|
|||
|
|
|
|||
|
|
if(dim)== 2
|
|||
|
|
H = [H ;perd_pr(:,i)'/norm(perd_pr(:,i)),zeros(1,13)];
|
|||
|
|
else
|
|||
|
|
H = [H ;perd_pr(:,i)'/norm(perd_pr(:,i)),zeros(1,12)];
|
|||
|
|
end
|
|||
|
|
Y = [Y ;norm(perd_pr(:,i))];
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|