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