ZhanLi/UWBIns
1
0
Fork 1
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
翟帅帅 2025-04-16 20:15:33 +08:00
commit fd57c4cc62
87 changed files with 4413 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitattributes vendored Normal file
View File

@ -0,0 +1 @@
study/eskf156/dataset/**/*.mat filter=lfs diff=lfs merge=lfs -text

55
.gitignore vendored Normal file
View File

@ -0,0 +1,55 @@
# Prerequisites
*.d
*.asv
*.bag
# *.csv
# Object files
*.o
*.ko
*.obj
*.elf
# Linker output
*.ilk
*.map
*.exp
# Precompiled Headers
*.gch
*.pch
# Libraries
*.lib
*.a
*.la
*.lo
# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib
# Executables
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex
# Debug files
*.dSYM/
*.su
*.idb
*.pdb
# Kernel Module Compile Results
*.mod*
*.cmd
.tmp_versions/
modules.order
Module.symvers
Mkfile.old
dkms.conf

38
README.md Normal file
View File

@ -0,0 +1,38 @@
# 基于超宽带(UWB)技术的扩展卡尔曼滤波(EKF)算法的 MATLAB 实现仓库
教学性质的：
* GPS IMU经典15维ESKF松组合
* VRU/AHRS姿态融合算法
* 捷联惯导速度位置姿态解算例子
* UWB IMU紧组合融合
* 每个例子自带数据集
运行环境：
* 最低版本: MATLAB R2022a, 必须安装sensor fusion toolbox和navigation tool box
* 需要将`\lib`及其子目录加入MATLAB预设目录， 或者运行一下根目录下的`init.m`
其中UWB+IMU融合和GPS+IMU融合就是经典的15维误差卡尔曼滤波(EKSF)，没有什么论文参考，就是一直用的经典的框架(就是松组合)，见参考部分。
更多内容请访问：
- 知乎：https://www.zhihu.com/people/yang-xi-97-90
- 网站：www.hipnuc.com
参考：
1. 书：捷联惯导算法与组合导航原理 严恭敏及PSINS工具箱官方网站：http://www.psins.org.cn/sy
2. 书：GNSS与惯性及多传感器组合导航系统原理 第二版
3. 书：GPS原理与接收机设计 谢刚
4. 深蓝学院-多传感器融合课程(理论推导及code)
5. 武汉大学牛小骥惯性导航课程(非常好，非常适合入门) https://www.bilibili.com/video/BV1nR4y1E7Yj
6. GPS IMU 松组合 https://kth.instructure.com/files/677996/download?download_frd=1
7. Coursera课程 https://www.coursera.org/learn/state-estimation-localization-self-driving-cars
推荐的学习路线：
1. 先看武汉大学惯性导航课程(牛小骥老师)，入门非常推荐，也不需要什么教材，做笔记
2. 看严恭敏老师的书籍，视频及code
3. 然后再入组合导航知识

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas1.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas2.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas3.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas4.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas5.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas6.mat Normal file
View File

Binary file not shown.

BIN
UWBInsFusion/Datasets/NTV/TWR-4A1T/datas7.mat Normal file
View File

Binary file not shown.

25
UWBInsFusion/README.md Normal file
View File

@ -0,0 +1,25 @@
数据集说明:
所有数据集均为实测机器人跑车数据，无仿真数据。
| 数据集 | 基站数 | 平面or3D | 说明 |
| ------ | ------ | -------- | ------------------------------------------------------------ |
| datas1 | 4 | 2D | 机器人U型跑车数据，机器人走走停停，速度不稳定 |
| datas2 | 4 | 2D | 机器人圆形轨迹跑车数据,机器人匀速运动，运动形状简单规则，可以用来初步验证算法 |
| datas3 | 4 | 2D | 机器人走一个典型赛道轨迹， 基本匀速，比较典型的实测数据 |
| datas4 | 4 | 2D | 相较于datas3 运动更复杂，场地更大. 机动更剧烈 |
| datas5 | 4 | 2D | 类似datas4 |
| datas5 | 4 | 2D | 类似datas4 |
数据集采集指导：
如果您自己第一次采集数据做实验，建议：
1. 建议IMU 100Hz, UWB 1-10Hz左右
2. 用小车或者汽车，不要用手拿着，避免很多随机机动。
3. 开始静止至少30s， 然后绕大8子，每个8子绕完不少于30s。 可以走走停停。做完一个比较标准(好识别)的动作后 静止几秒，再做下一个，比较典型的有八字，L型， 绕圈等
4. 运动结束后最好也停止30s。

107
UWBInsFusion/demo_plot.m Normal file
View File

@ -0,0 +1,107 @@
%% 说明
function demo_plot(dataset, out_data)
figure('NumberTitle', 'off', 'Name', '原始数据');
subplot(2, 2, 1);
plot(dataset.imu.acc');
legend("X", "Y", "Z");
title("加速度测量值");
subplot(2, 2, 2);
plot(dataset.imu.gyr');
legend("X", "Y", "Z");
title("陀螺测量值");
subplot(2, 2, 3);
plot(dataset.uwb.tof');
title("UWB测量值(伪距)");
subplot(2,2,4);
plot(diff(dataset.uwb.tof'));
title("伪距的差分(diff(tof))");
figure('NumberTitle', 'off', 'Name', '滤波器估计零偏');
subplot(2,1,1);
plot(out_data.delta_u(:,1:3));
legend("X", "Y", "Z");
title("加速度零偏");
subplot(2,1,2);
plot(rad2deg(out_data.delta_u(:,4:6)));
legend("X", "Y", "Z");
title("陀螺仪零偏");
figure('NumberTitle', 'off', 'Name', '量测滤波信息');
subplot(2,1,1);
plot(out_data.L);
title("量测置信度S");
figure('NumberTitle', 'off', 'Name', 'PVT');
subplot(2,2,1);
plot(out_data.x(:,1:3));
legend("X", "Y", "Z");
title("位置");
subplot(2,2,2);
plot(out_data.x(:,4:6));
legend("X", "Y", "Z");
title("速度");
subplot(2,2,3);
plot(out_data.eul);
legend("X", "Y", "Z");
title("欧拉角(姿态)");
figure('NumberTitle', 'off', 'Name', '硬件给出的UWB解算位置');
subplot(1,2,1);
plot3(out_data.uwb.pos(1,:), out_data.uwb.pos(2,:), out_data.uwb.pos(3,:), '.');
axis equal
title("UWB 伪距解算位置");
if(isfield(dataset, "pos"))
subplot(1,2,2);
plot3(dataset.pos(1,:), dataset.pos(2,:), dataset.pos(3,:), '.');
hold on;
plot3(out_data.x(:,1), out_data.x(:,2), out_data.x(:,3), '.-');
axis equal
title("硬件给出轨迹");
end
figure('NumberTitle', 'off', 'Name', '纯UWB伪距解算的位置和融合轨迹');
plot(out_data.uwb.pos(1,:), out_data.uwb.pos(2,:), '.');
hold on;
plot(out_data.uwb.fusion_pos(1,:), out_data.uwb.fusion_pos(2,:), '.-');
anch = out_data.uwb.anchor;
hold all;
scatter(anch(1, :),anch(2, :),'k');
for i=1:size(anch,2)
text(anch(1, i),anch(2, i),"A"+(i-1))
end
hold off;
legend("伪距解算UWB轨迹", "融合轨迹");
%
% if(isfield(dataset, "pos"))
% figure('NumberTitle', 'off', 'Name', '硬件给出的位置和融合轨迹');
% plot(dataset.pos(1,:), dataset.pos(2,:), '.');
% hold on;
% plot(out_data.uwb.fusion_pos(1,:), out_data.uwb.fusion_pos(2,:), '.-');
% legend("硬件给出轨迹", "融合轨迹");
% anch = out_data.uwb.anchor;
% hold all;
% scatter(anch(1, :),anch(2, :),'k');
% for i=1:size(anch,2)
% text(anch(1, i),anch(2, i),"A"+(i-1))
% end
% hold off;
% end

355
UWBInsFusion/eskf_uwb_imu.m Normal file
View File

@ -0,0 +1,355 @@
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

25
UWBInsFusion/uwb_imu_example_settings.m Normal file
View File

@ -0,0 +1,25 @@
function settings = uwb_imu_example_settings()
settings.sigma_uwb = 0.5; % UWB测距噪声
settings.sigma_acc = 0.02; %加速度计噪声
settings.sigma_gyro = deg2rad(0.3); %陀螺仪噪声
settings.sigma_acc_bias = 0.00; %加速度计零偏随机游走噪声
settings.sigma_gyro_bias = deg2rad(0.0); %陀螺仪零偏随机游走噪声
% Initial Kalman filter uncertainties (standard deviations)
settings.factp(1) = 20; % 初值位置方差(置信度)(m)，越大代表对初始位置越不信任
settings.factp(2) = 1; % 初值速度方差( [m/s]
settings.factp(3:5) = deg2rad([1 1 20]); % 初始i姿态方差 (roll,pitch,yaw) [rad]
settings.factp(6) = 0.01; % 初始加速度零偏方差 [m/s^2]
settings.factp(7) = deg2rad(0.01); % 初始角速度零偏方差 [rad/s]
end

25
lib/ENU2LLA.m Normal file
View File

@ -0,0 +1,25 @@
function [lat, lon, h] = ENU2LLA(E, N, U, lat0, lon0, h0)
% ENU 转 经纬高
% lat0, lon0, h0: 起始点经纬高, 经纬度为rad， 高度为m
% lat, lon, h 终点经纬度为rad， 高度为m
% E, N ,U 系下增量，单位为m
%精确算法
% XYZ0 = ch_LLA2ECEF(lat0, lon0, h0);
% XYZ1 = ch_LLA2ECEF(lat, lon, h);
% dXYZ = XYZ1 - XYZ0;
%
% [~, ~, C_ECEF2ENU, ~]= ch_earth(lat0, lon0, h0);
% dENU = C_ECEF2ENU * dXYZ;
% E = dENU(1);
% N= dENU(2);
% U = dENU(3);
%近似算法
R_0 = 6378137; %WGS84 Equatorial radius in meters
clat = cos(lat0);
h = h0 + U;
lon = lon0 + E/clat/R_0;
lat = lat0 + N/R_0 ;
end

108
lib/allan.m Normal file
View File

@ -0,0 +1,108 @@
%% ('NoiseDensity(角度随机游走)', N, 'RandomWalk(角速率随机游走)', K,'BiasInstability(零偏稳定性)', B);
% omega 必须为 rad/s(陀螺)， 加速度计: m/s^(2)
% see Freescale AN: Allan Variance: Noise Analysis for Gyroscopes
function [avar, tau, N, K, B] = allan(omega, Fs)
% t0 = 1/Fs;
% theta = cumsum(omega, 1)*t0;
%
% maxNumM = 200;
% L = size(theta, 1);
% maxM = 2.^floor(log2(L/2));
% m = logspace(log10(10), log10(maxM), maxNumM).';
% m = ceil(m); % m must be an integer.
% m = unique(m); % Remove duplicates.
%
% tau = m*t0;
%
% avar = zeros(numel(m), 1);
% for i = 1:numel(m)
% mi = m(i);
% avar(i,:) = sum( ...
% (theta(1+2*mi:L) - 2*theta(1+mi:L-mi) + theta(1:L-2*mi)).^2, 1);
% end
% avar = avar ./ (2*tau.^2 .* (L - 2*m));
[avar,tau] = allanvar(omega, 'octave', Fs);
adev = sqrt(avar);
%% Angle Random Walk
slope = -0.5;
logtau = log10(tau);
logadev = log10(adev);
dlogadev = diff(logadev) ./ diff(logtau);
[~, i] = min(abs(dlogadev - slope));
% Find the y-intercept of the line.
b = logadev(i) - slope*logtau(i);
% Determine the angle random walk coefficient from the line.
logN = slope*log10(1) + b;
N = 10^logN;
% Plot the results.
tauN = 1;
lineN = N ./ sqrt(tau);
%% Rate Random Walk
slope = 0.5;
logtau = log10(tau);
logadev = log10(adev);
dlogadev = diff(logadev) ./ diff(logtau);
[~, i] = min(abs(dlogadev - slope));
% Find the y-intercept of the line.
b = logadev(i) - slope*logtau(i);
% Determine the rate random walk coefficient from the line.
logK = slope*log10(3) + b;
K = 10^logK;
% Plot the results.
tauK = 3;
lineK = K .* sqrt(tau/3);
%% Bias Instability
slope = 0;
logtau = log10(tau);
logadev = log10(adev);
dlogadev = diff(logadev) ./ diff(logtau);
[~, i] = min(abs(dlogadev - slope));
[~, i] = min(adev); %yandld added
% Find the y-intercept of the line.
b = logadev(i) - slope*logtau(i);
% Determine the bias instability coefficient from the line.
scfB = sqrt(2*log(2)/pi);
logB = b - log10(scfB);
B = 10^logB;
% Plot the results.
tauB = tau(i);
lineB = B * scfB * ones(size(tau));
tauParams = [tauN, tauK, tauB];
params = [N, K, scfB*B];
figure;
loglog(tau, adev, '-*', tau, [lineN, lineK, lineB], '--', tauParams, params, 'o');
title('Allan Deviation with Noise Parameters')
xlabel('\tau')
ylabel('\sigma(\tau)')
legend('\sigma', '\sigma_N', '\sigma_K', '\sigma_B')
text(tauParams, params, {'N', 'K', '0.664B'})
grid on
end

12
lib/askew.m Normal file
View File

@ -0,0 +1,12 @@
function m = askew(v)
% 生成反对称矩阵
%
% Input: v - 3x1 vector
% Output: m - v的反对称阵
% | 0 -v(3) v(2) |
% m = | v(3) 0 -v(1) |
% |-v(2) v(1) 0 |
m = [ 0, -v(3), v(2);
v(3), 0, -v(1);
-v(2), v(1), 0 ];

44
lib/att_upt.m Normal file
View File

@ -0,0 +1,44 @@
function out = att_upt(in, gyr, dt)
% 输入当前姿态四元数和陀螺仪角速度，输出更新后的姿态四元数
% in为当前姿态四元数
% gyr是陀螺仪测量值
% dt 时间步长
%% 单子样旋转矢量
rv = gyr*dt; % 旋转矢量 = 角速度 × 时间步长
dq = rv2q(rv); % 调用函数将旋转矢量转为四元数
%% 不专业的做法
% dq(1) = 1;
% dq(2) = rv(1)*0.5;
% dq(3) = rv(2)*0.5;
% dq(4) = rv(3)*0.5;
out = qmul(in, dq); % 四元数相乘 当前姿态 × 增量 = 新姿态
out = qnormlz(out); % 归一化 归一化防止数值漂移
%% 使用旋转矩阵更新
%
% Cb2n = ch_q2m(in);
% theta = gyr*dt;
%
% %C = eye(3) + ch_askew(theta);
% C = ch_rv2m(theta);
%
% Cb2n = Cb2n * C;
%
% % 截断误差，保持正交化 GNSS与惯性及多传感器组合导航系统原理-第二版.pdf 公式 5.80
% c1 = Cb2n(1,:);
% c2 = Cb2n(2,:);
% c3 = Cb2n(3,:);
% c1 = 2 / (1 + dot(c1,c1))*c1;
% c2 = 2 / (1 + dot(c2,c2))*c2;
% c3 = 2 / (1 + dot(c3,c3))*c3;
% Cb2n = [c1; c2; c3];
%
% out = ch_m2q(Cb2n);
end

34
lib/calbiration/acc_calibration.m Normal file
View File

@ -0,0 +1,34 @@
%% ACC Calibraiton
function [C, B] = acc_calibration(input)
%% OPTION1: ST mehold AN4508 Application note Parameters and calibration of a low-g 3-axis accelerometer
B = [1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1];
A= [input -[ones(length(input), 1)]];
X = inv(A'*A) * A'*B;
C = X(1:3,:)';
B = X(4,:)';
%% OPTION2: Time- and Computation-Efficient Calibration of MEMS 3D Accelerometers and Gyroscopes
% Data: M x N M: sample set(6), N:acc X,Y,Z (3)
% row1 = 1, 0, 0 case
% row2 = 0, 1, 0 case
% row3 = 0, 0, 1 case
% row4 = -1, 0, 0 case
% row5 = 0, -1, 0 case
% row6 = 0, 0, -1 case
% input = input';
% Asp = input(:,1:3);
% Asn = input(:,4:6);
%
% B = ((Asp + Asn)*[1 1 1]' / 6);
% C = 2*(Asp - Asn)^(-1);
end

112
lib/calbiration/ch_magcal2d.m Normal file
View File

@ -0,0 +1,112 @@
clear;
clc;
close all;
% %% http://ztrw.mchtr.pw.edu.pl/en/least-squares-circle-fit/
%
%
% input = [
% 9.1667 0.5000 1.0000
% 0.3333 1.8750 1.0000
% -7.8083 7.4167 1.0000
% -10.0167 11.2500 1.0000
% -15.5583 21.3750 1.0000
% -16.7500 31.6250 1.0000
% -13.4333 40.8333 1.0000
% 4.3917 53.0000 1.0000
% 15.3500 54.8750 1.0000
% 21.3083 54.6250 1.0000
% 32.5417 49.2083 1.0000
% 33.0417 38.8333 1.0000
% 32.8750 31.5417 1.0000
% 34.3083 19.3750 1.0000
% 25.2917 11.0417 1.0000
% 16.2500 5.0000 1.0000
% 11.2083 4.0000 1.0000
% ];
%
%
% P = input(:,1:2)';
% n = length(P);
clear;
clc;
close all;
P = [1 7; 2 6; 5 8; 7 7; 9 5; 3 7]';
n= length(P);
plot(P(1,:), P(2,:), '*');
%build deisgn matrix
A = [ P(1,:); P(2,:); ones(1,n)]';
b = sum(P.*P, 1)';
% ls solution
a= (A'*A)^(-1)*A'*b;
xc = 0.5*a(1);
yc = 0.5*a(2);
R = sqrt((a(1)^2+a(2)^2)/4+a(3));
R
viscircles([xc, yc],R);
axis equal
% max_min_ofs = max(P,[],2) + min(P, [], 2) ;
% max_min_ofs = max_min_ofs / 2;
% max_min_ofs
% %¸ø¨³õÖµ
% xc = 5.3794;
% yc = 7.2532;
% r = 3.0370;
% res = [xc; yc; r];
%
% max_iters = 20;
%
% max_dif = 10^(-6); % max difference between consecutive results
% for i = 1 : max_iters
% J = Jacobian(res(1), res(2), P);
% R = Residual(res(1), res(2), res(3), P);
% prev = res;
% res = res - (J'*J)^(-1)*J'*R;
% dif = abs((prev - res)./res);
% if dif < max_dif
% fprintf('Convergence met after %d iterations\n', i);
% break;
% end
% end
% if i == max_iters
% fprintf('Convergence not reached after %d iterations\n', i);
% end
%
% xc = res(1);
% yc = res(2);
% r = res(3);
%
% plot(P(:,1), P(:,2), '*')
% viscircles([xc, yc],r);
% axis equal
%
% function J = Jacobian(xc, yc, P)
% len = size(P);
% r = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2);
% J = [(xc - P(:,1))./r, (yc - P(:,2))./r, -ones(len(1), 1)];
% end
function R = Residual(xc, yc, r, P)
R = sqrt((xc - P(:,1)).^2 + (yc - P(:,2)).^2) - r;
end
%

185
lib/calbiration/ellipsoid_fit.m Normal file
View File

@ -0,0 +1,185 @@
function [A, b, magB, er]=ellipsoid_fit(XYZ,varargin)
% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points
% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z)
% optional flag f, default to 0 (fitting of rotated ellipsoid)
A = eye(3);
x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3);
if nargin>1
f=varargin{1};
else f=0;
end
if( f == 5)
[A, b, magB, er, ispd] = correctEllipsoid4(x,y,z);
end
if( f == 1)
[A, b, magB, er, ispd] = correctEllipsoid7(x,y,z);
end
if( f == 0)
[A, b, magB, er, ispd] = correctEllipsoid10(x,y,z);
end
end
function [Winv, V, B, er, ispd] = correctEllipsoid4(x,y,z)
% R is the identity
b = x.*x + y.*y + z.*z;
A = [x,y,z];
A = [A ones(numel(x),1)];
soln = (A'*A)^(-1)*A'*b;
Winv = eye(3);
V = 0.5*soln(1:3);
B = sqrt(soln(4) + sum(V.*V));
% res = residual(Winv, V, B, [x,y,z])
% er = (1/(2*B*B))*sqrt(res.'*res/numel(x));
if nargout > 3
res = A*soln - b;
er = (1/(2*B*B) * sqrt( res.'*res / numel(x)));
ispd = 1;
else
er = -ones(1);
ispd = -1;
end
end
function [Winv, V, B, er, ispd] = correctEllipsoid7(x,y,z)
d = [x.*x, y.*y, z.*z, x, y, z, ones(size(x))];
dtd = d.' * d;
[evc, evlmtx] = eig(dtd);
eigvals = diag(evlmtx);
[~, idx] = min(eigvals);
beta = evc(:,idx); %solution has smallest eigenvalue
A = diag(beta(1:3));
dA = det(A);
if dA < 0
A = -A;
beta = -beta;
dA = -dA; %Compensate for -A.
end
V = -0.5*(beta(4:6)./beta(1:3)); %hard iron offset
B = sqrt(abs(sum([...
A(1,1)*V(1)*V(1), ...
A(2,2)*V(2)*V(2), ...
A(3,3)*V(3)*V(3), ...
-beta(end)] ...
)));
% We correct Winv and B by det(A) because we don't know which has the
% gain. By convention, normalize A.
det3root = nthroot(dA,3);
det6root = sqrt(det3root);
Winv = sqrtm(A./det3root);
B = B./det6root;
if nargout > 3
res = residual(Winv,V,B, [x,y,z]);
er = (1/(2*B*B))*sqrt(res.'*res/numel(x));
[~,p] = chol(A);
ispd = (p == 0);
else
er = -ones(1, 'like',x);
ispd = -1;
end
end
function [Winv, V,B,er, ispd] = correctEllipsoid10(x,y,z)
d = [...
x.*x, ...
2*x.*y, ...
2*x.*z, ...
y.*y, ...
2*y.*z, ...
z.*z, ...
x, ...
y, ...
z, ...
ones(size(x))];
dtd = d.' * d;
[evc, evlmtx] = eig(dtd);
eigvals = diag(evlmtx);
[~, idx] = min(eigvals);
beta = evc(:,idx); %solution has smallest eigenvalue
A = beta([1 2 3; 2 4 5; 3 5 6]); %make symmetric
dA = det(A);
if dA < 0
A = -A;
beta = -beta;
dA = -dA; %Compensate for -A.
end
V = -0.5*(A^(-1)*beta(7:9)); %hard iron offset
B = sqrt(abs(sum([...
A(1,1)*V(1)*V(1), ...
2*A(2,1)*V(2)*V(1), ...
2*A(3,1)*V(3)*V(1), ...
A(2,2)*V(2)*V(2), ...
2*A(3,2)*V(2)*V(3), ...
A(3,3)*V(3)*V(3), ...
-beta(end)] ...
)));
% We correct Winv and B by det(A) because we don't know which has the
% gain. By convention, normalize A.
det3root = nthroot(dA,3);
det6root = sqrt(det3root);
Winv = sqrtm(A./det3root);
B = B./det6root;
if nargout > 3
res = residual(Winv,V,B, [x,y,z]);
er = (1/(2*B*B))*sqrt(res.'*res/numel(x));
[~,p] = chol(A);
ispd = (p == 0);
else
er = -ones(1, 'like',x);
ispd = -1;
end
end
function r = residual(Winv, V, B, data)
% Residual error after correction
spherept = (Winv * (data.' - V)).'; % a point on the unit sphere
radsq = sum(spherept.^2,2);
r = radsq - B.^2;
end

207
lib/calbiration/ellipsoid_fit_new.m Normal file
View File

@ -0,0 +1,207 @@
function [ center, radii, evecs, v, chi2 ] = ellipsoid_fit_new( X, equals )
%
% Fit an ellispoid/sphere/paraboloid/hyperboloid to a set of xyz data points:
%
% [center, radii, evecs, pars ] = ellipsoid_fit( X )
% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
%
% Parameters:
% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors
% * flag - '' or empty fits an arbitrary ellipsoid (default),
% - 'xy' fits a spheroid with x- and y- radii equal
% - 'xz' fits a spheroid with x- and z- radii equal
% - 'xyz' fits a sphere
% - '0' fits an ellipsoid with its axes aligned along [x y z] axes
% - '0xy' the same with x- and y- radii equal
% - '0xz' the same with x- and z- radii equal
%
% Output:
% * center - ellispoid or other conic center coordinates [xc; yc; zc]
% * radii - ellipsoid or other conic radii [a; b; c]
% * evecs - the radii directions as columns of the 3x3 matrix
% * v - the 10 parameters describing the ellipsoid / conic algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz + J = 0
% * chi2 - residual sum of squared errors (chi^2), this chi2 is in the
% coordinate frame in which the ellipsoid is a unit sphere.
%
% Author:
% Yury Petrov, Oculus VR
% Date:
% September, 2015
%
%reference
narginchk( 1, 3 ) ; % check input arguments
if nargin == 1
equals = ''; % no constraints by default
end
if size( X, 2 ) ~= 3
error( 'Input data must have three columns!' );
else
x = X( :, 1 );
y = X( :, 2 );
z = X( :, 3 );
end
% need nine or more data points
if length( x ) < 9 && strcmp( equals, '' )
error( 'Must have at least 9 points to fit a unique ellipsoid' );
end
if length( x ) < 8 && ( strcmp( equals, 'xy' ) || strcmp( equals, 'xz' ) )
error( 'Must have at least 8 points to fit a unique ellipsoid with two equal radii' );
end
if length( x ) < 6 && strcmp( equals, '0' )
error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
end
if length( x ) < 5 && ( strcmp( equals, '0xy' ) || strcmp( equals, '0xz' ) )
error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two equal radii' );
end
if length( x ) < 4 && strcmp( equals, 'xyz' );
error( 'Must have at least 4 points to fit a unique sphere' );
end
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx +
% 2Hy + 2Iz + J = 0 and A + B + C = 3 constraint removing one extra
% parameter
if strcmp( equals, '' )
D = [ x .* x + y .* y - 2 * z .* z, ...
x .* x + z .* z - 2 * y .* y, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 9 ellipsoid parameters
elseif strcmp( equals, 'xy' )
D = [ x .* x + y .* y - 2 * z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 8 ellipsoid parameters
elseif strcmp( equals, 'xz' )
D = [ x .* x + z .* z - 2 * y .* y, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 8 ellipsoid parameters
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
elseif strcmp( equals, '0' )
D = [ x .* x + y .* y - 2 * z .* z, ...
x .* x + z .* z - 2 * y .* y, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 6 ellipsoid parameters
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
% where A = B or B = C or A = C
elseif strcmp( equals, '0xy' )
D = [ x .* x + y .* y - 2 * z .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 5 ellipsoid parameters
elseif strcmp( equals, '0xz' )
D = [ x .* x + z .* z - 2 * y .* y, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 5 ellipsoid parameters
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
elseif strcmp( equals, 'xyz' )
D = [ 2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 4 ellipsoid parameters
else
error( [ 'Unknown parameter value ' equals '!' ] );
end
% solve the normal system of equations
d2 = x .* x + y .* y + z .* z; % the RHS of the llsq problem (y's)
u = ( D' * D ) \ ( D' * d2 ); % solution to the normal equations
% find the residual sum of errors
% chi2 = sum( ( 1 - ( D * u ) ./ d2 ).^2 ); % this chi2 is in the coordinate frame in which the ellipsoid is a unit sphere.
% find the ellipsoid parameters
% convert back to the conventional algebraic form
if strcmp( equals, '' )
v(1) = u(1) + u(2) - 1;
v(2) = u(1) - 2 * u(2) - 1;
v(3) = u(2) - 2 * u(1) - 1;
v( 4 : 10 ) = u( 3 : 9 );
elseif strcmp( equals, 'xy' )
v(1) = u(1) - 1;
v(2) = u(1) - 1;
v(3) = -2 * u(1) - 1;
v( 4 : 10 ) = u( 2 : 8 );
elseif strcmp( equals, 'xz' )
v(1) = u(1) - 1;
v(2) = -2 * u(1) - 1;
v(3) = u(1) - 1;
v( 4 : 10 ) = u( 2 : 8 );
elseif strcmp( equals, '0' )
v(1) = u(1) + u(2) - 1;
v(2) = u(1) - 2 * u(2) - 1;
v(3) = u(2) - 2 * u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 3 : 6 )' ];
elseif strcmp( equals, '0xy' )
v(1) = u(1) - 1;
v(2) = u(1) - 1;
v(3) = -2 * u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 2 : 5 )' ];
elseif strcmp( equals, '0xz' )
v(1) = u(1) - 1;
v(2) = -2 * u(1) - 1;
v(3) = u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 2 : 5 )' ];
elseif strcmp( equals, 'xyz' )
v = [ -1 -1 -1 0 0 0 u( 1 : 4 )' ];
end
v = v';
% form the algebraic form of the ellipsoid
A = [ v(1) v(4) v(5) v(7); ...
v(4) v(2) v(6) v(8); ...
v(5) v(6) v(3) v(9); ...
v(7) v(8) v(9) v(10) ];
% find the center of the ellipsoid
center = -A( 1:3, 1:3 ) \ v( 7:9 );
% form the corresponding translation matrix
T = eye( 4 );
T( 4, 1:3 ) = center';
% translate to the center
R = T * A * T';
% solve the eigenproblem
[ evecs, evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
radii = sqrt( 1 ./ diag( abs( evals ) ) );
sgns = sign( diag( evals ) );
radii = radii .* sgns;
% calculate difference of the fitted points from the actual data normalized by the conic radii
d = [ x - center(1), y - center(2), z - center(3) ]; % shift data to origin
d = d * evecs; % rotate to cardinal axes of the conic;
d = [ d(:,1) / radii(1), d(:,2) / radii(2), d(:,3) / radii(3) ]; % normalize to the conic radii
chi2 = sum( abs( 1+0*x - sum( d.^2, 2 ) ) );
if abs( v(end) ) > 1e-6
v = -v / v(end); % normalize to the more conventional form with constant term = -1
else
v = -sign( v(end) ) * v;
end

29
lib/calbiration/gyr_calibration.m Normal file
View File

@ -0,0 +1,29 @@
%% GYR Calibraiton
% example:
% input = [
% 65.2191 0.1712 0.4618
% -0.0901 71.9079 -0.0728
% -0.5425 -0.1436 71.7273
% -65.0873 -0.1008 -0.4814
% 0.1573 -71.9432 0.0436
% 0.6128 0.2009 -71.7076
% ];
%
% theory = [65.5618 0 0; 0 72.0649 0; 0 0 72.1298 ; -65.5081 0 0; 0 -72.1298 0; 0 0 -72.0649];
% gyr_calibration(input, theory)
function [C, B] = gyr_calibration(input, ref)
%% ST mehold AN4508 Application note Parameters and calibration of a low-g 3-axis accelerometer
A= [input -[ones(length(input), 1)]];
B = ref;
X = inv(A'*A) * A'*B;
C = X(1:3,:)';
B = X(4,:)';
%% Time- and Computation-Efficient Calibration of MEMS 3D Accelerometers and Gyroscopes
% TBD
end

53
lib/data_import.m Normal file
View File

@ -0,0 +1,53 @@
function dataset = data_import(path)
tbl = readtable(path);
if sum(ismember(tbl.Properties.VariableNames,'AccX'))
dataset.imu.acc(1,:) = tbl.AccX';
end
if sum(ismember(tbl.Properties.VariableNames,'AccY'))
dataset.imu.acc(2,:) = tbl.AccY';
end
if sum(ismember(tbl.Properties.VariableNames,'AccZ'))
dataset.imu.acc(3,:) = tbl.AccZ';
end
if sum(ismember(tbl.Properties.VariableNames,'GyrX'))
dataset.imu.gyr(1,:) = tbl.GyrX';
end
if sum(ismember(tbl.Properties.VariableNames,'GyrY'))
dataset.imu.gyr(2,:) = tbl.GyrY';
end
if sum(ismember(tbl.Properties.VariableNames,'GyrZ'))
dataset.imu.gyr(3,:) = tbl.GyrZ';
end
if sum(ismember(tbl.Properties.VariableNames,'MagX'))
dataset.imu.mag(1,:) = tbl.MagX';
end
if sum(ismember(tbl.Properties.VariableNames,'MagY'))
dataset.imu.mag(2,:) = tbl.MagY';
end
if sum(ismember(tbl.Properties.VariableNames,'MagZ'))
dataset.imu.mag(3,:) = tbl.MagZ';
end
if sum(ismember(tbl.Properties.VariableNames,'Roll'))
dataset.eul.roll = tbl.Roll';
end
if sum(ismember(tbl.Properties.VariableNames,'Pitch'))
dataset.eul.pitch = tbl.Pitch';
end
if sum(ismember(tbl.Properties.VariableNames,'Yaw'))
dataset.eul.yaw = tbl.Yaw';
end

138
lib/dip13.m Normal file
View File

@ -0,0 +1,138 @@
% syms b1 b2 b3 real
% syms g1 g2 g3 real
% syms v1 v2 v3 real
% syms lambda real
% syms h
% syms l11 l12 l13 l21 l22 l23 l31 l32 l33 real
% L = [l11 l23 l13; l21 l22 l23; l31 l32 l33];
% B = [b1 b2 b3]';
% V = [v1 v2 v3]';
% G = [g1 g2 g3]';
%
% f = V'*L'*L*V - 2*V'*L'*L*B + B'*L'*L*B - h^2
% j = jacobian(f, [l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3 lamda]);
% collect(j(1,1), [l11])
%
% alp = L*(V - B);
% beta = V-B;
% 2*alp(1)*beta(1);
% collect(ans,[l11])
function [mis, bias, lamda, inter, residual] = dip13 (acc, mag, mag_norm, epsilon)
%函数的功能：using DIP(dual inner product mehold to caluate mag)
%函数的描述： Calibration of tri-axial magnetometer using vector observations and inner products or Calibration and Alignment of Tri-Axial Magnetometers for Attitude Determination
%函数的使用：[mis, bias, lamda, inter, J] = dip (acc, mag, mag_norm, epsilon)
%输入：
% input1: acc: raw acc
% input2: mag: raw mag
%输出：
% mis: misalign matrix
% bias: bias
% lamda: |reference_vector| * |mag_vector| * cos(theta)
% inter: interation
% J: cost function array
mis = eye(3);
bias = zeros(1,3);
lamda = cos(deg2rad(60));
mu = 0.001; % damp
inter = 100;
last_e = 100;
last_J = 100;
% [l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3 lamda], lamba = cos(inclincation)
x = [1 0 0 0 1 0 0 0 1 0 0 0 1];
% using max-min mehold to get a inital bias
x(10:12) = (max(mag) + min(mag)) / 2;
[last_J, ~, ~] = lm_dip(x, mag, acc, mag_norm);
for i = 1:inter
[residual(i), Jacobi, e] = lm_dip(x, mag, acc, mag_norm);
x = x - (inv((Jacobi' * Jacobi + mu * eye(length(x)))) * Jacobi' * e)';
%x = x - mu*(Jacobi' * e)';
if(residual(i) <= last_J)
mu = 0.1 * mu;
else
mu = 10 * mu;
end
if((abs(norm(e) - norm(last_e))) < epsilon)
mis =[x(1:3); x(4:6); x(7:9)];
bias = x(10:12)';
lamda = x(13);
inter = i;
break;
end
last_e = e;
last_J = residual(i);
end
end
%% Function
% Jval: error
% gradient : gradient of cost func
% e: Fx
function[residual, gradient, e]=lm_dip(x, mB, gB, h)
n = length(mB);
e = zeros(n*2, 1);
gradient = zeros(n*2, 13);
L =[x(1:3); x(4:6); x(7:9)];
b = x(10:12)';
for i = 1:n
v = mB(i, :)';
g = gB(i , :)';
% caluate e
e(i , :) = v'*L'*L*v -2*v'*L'*L*b + b'*L'*L*b - h^2;
% e(i , :) = (L*(v - b))' * (L*(v - b)) - h^2;
% e(n+ i, :) = g'*L*v - g'*L*b - h*norm(g)*x(13);
e(n+ i, :) = g'*L*(v-b) - h*norm(g)*x(13);
alpa = L * (v - b);
beta = v - b;
% caluate J 1- r
gradient(i, 1) = 2 * alpa(1) * beta(1);
gradient(i, 2) = 2 * alpa(1) * beta(2);
gradient(i, 3) = 2 * alpa(1) * beta(3);
gradient(i, 4) = 2 * alpa(2) * beta(1);
gradient(i, 5) = 2 * alpa(2) * beta(2);
gradient(i, 6) = 2 * alpa(2) * beta(3);
gradient(i, 7) = 2 * alpa(3) * beta(1);
gradient(i, 8) = 2 * alpa(3) * beta(2);
gradient(i, 9) = 2 * alpa(3) * beta(3);
gradient(i, 10) = -2 * (alpa(1) * L(1,1) + alpa(2) * L(2,1) + alpa(3) * L(3,1));
gradient(i, 11) = -2 * (alpa(1) * L(1,2) + alpa(2) * L(2,2) + alpa(3) * L(3,2));
gradient(i, 12) = -2 * (alpa(1) * L(1,3) + alpa(2) * L(2,3) + alpa(3) * L(3,3));
gradient(i, 13) = 0;
% caluate J r- 2r
gradient(n + i, 1) = g(1) * beta(1);
gradient(n + i, 2) = g(1) * beta(2);
gradient(n + i, 3) = g(1) * beta(3);
gradient(n + i, 4) = g(2) * beta(1);
gradient(n + i, 5) = g(2) * beta(2);
gradient(n + i, 6) = g(2) * beta(3);
gradient(n + i, 7) = g(3) * beta(1);
gradient(n + i, 8) = g(3) * beta(2);
gradient(n + i, 9) = g(3) * beta(3);
gradient(n + i, 10) = - (g(1) * L(1,1) + g(2) * L(2,1) + g(3) * L(3,1));
gradient(n + i, 11) = - (g(1) * L(1,2) + g(2) * L(2,2) + g(3) * L(3,2));
gradient(n + i, 12) = - (g(1) * L(1,3) + g(2) * L(2,3) + g(3) * L(3,3));
gradient(n + i, 13) = -norm(v) * norm(g);
end
residual = e' * e;
end

98
lib/dip5.m Normal file
View File

@ -0,0 +1,98 @@
% using DIP mehold, only compute bias, norm_h, lamda(cos(inclination)),
% modifed from DIP13
% %% moudle DIP5
% syms b1 b2 b3 real
% syms g1 g2 g3 real
% syms v1 v2 v3 real
% syms lambda real
% syms norm_h norm_g real
% B = [b1 b2 b3]';
% V = [v1 v2 v3]';
% G = [g1 g2 g3]';
% % %G'*(V - B) - CONST
%
% f1= (V - B)' * (V - B) - norm_h^2;
% j1 = jacobian(f1, [B' norm_h lambda]);
% collect(j1, B) ;
%
% f2 = G' * (V - B) - norm_g * norm_h * lambda;
%
% f = f1 + f2;
% j2 = jacobian(f2, [B' norm_h lambda]);
% collect(j2, B);
%
%
% f1
% j1
%
% f2
% j2
function [mis, bias, norm_h, lamda, inter, J] = dip5 (acc, mag, x, epsilon)
% initalize value
mis = eye(3);
bias = zeros(1,3);
lamda = cos(deg2rad(60));
mu = 0.000; % damp
inter = 100;
last_e = 100;
for i = 1:inter
[J(i), Jacobi, e] = compute_cost_and_jacob (x, mag, acc);
x = x - (inv((Jacobi' * Jacobi + mu * eye(length(x)))) * Jacobi' * e)';
if((abs(norm(e) - norm(last_e))) < epsilon)
bias = x(1:3)';
lamda = x(5);
inter = i;
norm_h = x(4);
break;
end
last_e = e;
end
end
%% Function
% residual: error
% J : gradient of cost func
% e: bx by bz lamada
% X: [Bx By Bz norm_h lambada]
function[residual, J, e]=compute_cost_and_jacob(x, mB, gB)
n = length(mB);
e = zeros(n * 2, 1);
J = zeros(n * 2, 5);
b = x(1:3)';
norm_h = x(4);
lamda = x(5);
for i = 1:n
v = mB(i, :)';
g = gB(i , :)';
% caluate e
e(i , :) = (v - b)' * (v - b) - norm_h^2;
e(n+ i, :) = g' * (v - b) - (norm_h * norm(g) * lamda);
% caluate J 1- r
J(i, 1) = 2 * (b(1) - v(1));
J(i, 2) = 2 * (b(2) - v(2));
J(i, 3) = 2 * (b(3) - v(3));
J(i, 4) = -2 * norm_h;
J(i, 5) = 0;
% caluate J r- 2r
J(n + i, 1) = -g(1);
J(n + i, 2) = -g(2);
J(n + i, 3) = -g(3);
J(n + i, 4) = -lamda * norm(g);
J(n + i, 5) = -norm_h * norm(g);
end
residual = e' * e;
end

57
lib/dpi.m Normal file
View File

@ -0,0 +1,57 @@
%% A new calibration method for tri-axial field sensors in strap-down navigation systems µã»ý²»±ä·¨
% Accelerometer: acc is sensor frame, n= number of obs, m = 3(x y z)
% Magnetometer: raw mag in sensor frame
% A: mag misalignment matrix
% b: mag bias
function [mis bias] = dpi (acc, mag, dot_product)
nbos = length(acc);
% x = l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3
A(:,1) = acc(:,1).* mag(:,1);
A(:,2) = acc(:,1).* mag(:,2);
A(:,3) = acc(:,1).* mag(:,3);
A(:,4) = acc(:,2).* mag(:,1);
A(:,5) = acc(:,2).* mag(:,2);
A(:,6) = acc(:,2).* mag(:,3);
A(:,7) = acc(:,3).* mag(:,1);
A(:,8) = acc(:,3).* mag(:,2);
A(:,9) = acc(:,3).* mag(:,3);
A(:,10) = -acc(:,1);
A(:,11) = -acc(:,2);
A(:,12) = -acc(:,3);
%Y = ones(length(gB),1) * dot(gmOb, gReference);
Y = ones(nbos,1) * dot_product;
B = inv(A'*A)*A'*Y;
% or you can use lsqlin(A,Y)
mis = [B(1:3)'; B(4:6)'; B(7:9)'];
bias = [B(10) B(11) B(12)];
end
%% DPI module
% syms l11 l12 l13 l21 l22 l23 l31 l32 l33 real
% syms b1 b2 b3 real
% syms g1 g2 g3 real
% syms v1 v2 v3 real
% syms CONST real
% b: bias
% l11 - l33: mis align
% v magnormetor reading
% g graivity
% model
% L = sym('l%d%d', [3 3]);
% B = [b1 b2 b3]';
% V = [v1 v2 v3]';
% G = [g1 g2 g3]';
% G'*(L*V - B) - CONST
% %G'*(V - B) - CONST
%
% collect(ans, [L B])

8
lib/dv2atti.m Normal file
View File

@ -0,0 +1,8 @@
function [Cb2n] = dv2atti(vn1, vn2, vb1, vb2)
vntmp1 = cross(vn1,vn2); vntmp2 = cross(vntmp1,vn1);
vbtmp1 = cross(vb1,vb2); vbtmp2 = cross(vbtmp1,vb1);
Cb2n = [vn1/norm(vn1), vntmp1/norm(vntmp1), vntmp2/norm(vntmp2)]*...
[vb1/norm(vb1), vbtmp1/norm(vbtmp1), vbtmp2/norm(vbtmp2)]';

11
lib/geo/ch_ECEF2ENU.m Normal file
View File

@ -0,0 +1,11 @@
function [E, N, U] = ch_ECEF2ENU(XYZ, lat0, lon0, h0)
% ECEF坐标转ENU
% lat0, lon0, h0: 起始点经纬高, 经纬度为rad， 高度为m
% XYZ ECEF坐标 单位为m
% ENU距离 单位为m
[lat, lon, h] = ch_ECEF2LLA(XYZ);
[E, N, U] = ch_LLA2ENU(lat, lon, h, lat0, lon0, h0);
end

46
lib/geo/ch_ECEF2LLA.m Normal file
View File

@ -0,0 +1,46 @@
function [lat, lon, h] = ch_ECEF2LLA(XYZ)
% ECEF坐标转经纬高
% lat:纬度(rad)
% lon:经度(rad)
% h高度(m)
R_0 = 6378137; %WGS84 Equatorial radius in meters
e = 0.0818191908425; %WGS84 eccentricity
lon = atan2(XYZ(2),XYZ(1));
% From (C.29) and (C.30)
k1 = sqrt(1 - e^2) * abs (XYZ(3));
k2 = e^2 * R_0;
beta = sqrt(XYZ(1)^2 + XYZ(2)^2);
E = (k1 - k2) / beta;
F = (k1 + k2) / beta;
% From (C.31)
P = 4/3 * (E*F + 1);
% From (C.32)
Q = 2 * (E^2 - F^2);
% From (C.33)
D = P^3 + Q^2;
% From (C.34)
V = (sqrt(D) - Q)^(1/3) - (sqrt(D) + Q)^(1/3);
% From (C.35)
G = 0.5 * (sqrt(E^2 + V) + E);
% From (C.36)
T = sqrt(G^2 + (F - V * G) / (2 * G - E)) - G;
% From (C.37)
lat = sign(XYZ(3)) * atan((1 - T^2) / (2 * T * sqrt (1 - e^2)));
% From (C.38)
h = (beta - R_0 * T) * cos(lat) +...
(XYZ(3) - sign(XYZ(3)) * R_0 * sqrt(1 - e^2)) * sin (lat);
end

10
lib/geo/ch_ENU2ECEF.m Normal file
View File

@ -0,0 +1,10 @@
function XYZ = ch_ENU2ECEF(E, N, U, lat0, lon0, h0)
% ENU 转 ECEF
% lat0, lon0, h0: 起始点经纬高, 经纬度为rad， 高度为m
% ENU东北天距离，单位为m
% 返回 ECEF坐标 单位m
[lat, lon, h] = ch_ENU2LLA(E, N, U, lat0, lon0, h0);
XYZ = ch_LLA2ECEF(lat, lon, h);
end

30
lib/geo/ch_ENU2LLA.m Normal file
View File

@ -0,0 +1,30 @@
function [lat, lon, h] = ch_ENU2LLA(E, N ,U, lat0, lon0, h0)
% ENU 转 经纬高
% E, N ,U 系下增量，单位为m
% lat0, lon0, h0: 起始点经纬高, 经纬度为rad， 高度为m
% lat, lon, h 终点经纬高, 经纬度为rad， 高度为m
%精确解
XYZ0 = ch_LLA2ECEF(lat0, lon0, h0);
[~, ~, C_ECEF2ENU, ~]= ch_earth(lat0, lon0, h0);
dXYZ = C_ECEF2ENU' * [E N U]';
XYZ = dXYZ + XYZ0;
[lat, lon, h] = ch_ECEF2LLA(XYZ);
% % 近似算法
% R_0 = 6378137; %WGS84 Equatorial radius in meters
% clat = cos(lat0);
%
% dlon = E / (R_0 * clat);
% dlat = N / R_0;
% dh = U;
%
% lon = lon0 + dlon;
% lat = lat0 + dlat;
% h = h0 + dh;
end

24
lib/geo/ch_LLA2ECEF.m Normal file
View File

@ -0,0 +1,24 @@
function [XYZ] = ch_LLA2ECEF(lat, lon, h)
% 经纬高转ECEF坐标
% lat:纬度(rad)
% lon:经度(rad)
% h高度(m)
R0 = 6378137; %WGS84 Equatorial radius in meters
e = 0.0818191908425; %WGS84 eccentricity
% Calculate transverse radius of curvature using (2.105)
RN = R0 / sqrt(1 - (e * sin(lat))^2);
% Convert position using (2.112)
clat = cos(lat);
slat = sin(lat);
clon = cos(lon);
slon = sin(lon);
XYZ = [0 0 0]';
XYZ(1) = (RN + h) * clat * clon;
XYZ(2) = (RN + h) * clat * slon;
XYZ(3) = ((1 - e^2) * RN + h) * slat;

25
lib/geo/ch_LLA2ENU.m Normal file
View File

@ -0,0 +1,25 @@
function [E, N, U] = ch_LLA2ENU(lat, lon, h, lat0, lon0, h0)
% 经纬高 转 ENU
% lat0, lon0, h0: 起始点经纬高, 经纬度为rad， 高度为m
% lat, lon, h 终点经纬高, 经纬度为rad， 高度为m
% E, N ,U 系下增量，单位为m
%精确算法
% XYZ0 = ch_LLA2ECEF(lat0, lon0, h0);
% XYZ1 = ch_LLA2ECEF(lat, lon, h);
% dXYZ = XYZ1 - XYZ0;
%
% [~, ~, C_ECEF2ENU, ~]= ch_earth(lat0, lon0, h0);
% dENU = C_ECEF2ENU * dXYZ;
% E = dENU(1);
% N= dENU(2);
% U = dENU(3);
%近似算法
R_0 = 6378137; %WGS84 Equatorial radius in meters
clat = cos(lat0);
E = (lon - lon0) * clat * R_0;
N = (lat - lat0) * R_0;
U = h - h0;
end

40
lib/geo/ch_earth.m Normal file
View File

@ -0,0 +1,40 @@
function [Rns, Rew, C_ECEF2ENU, C_ECEF2NED]= ch_earth(lat, lon, h)
%% 根据经纬度计算地球常用参数
% INPUT
% lat: 纬度(rad)
% lon: 经度(rad)
% OUTPUT
% Rns, R_meridian(RM, ns)： 南北向地球曲率半径, 子午圈曲率半径(竖着的)
% Rew_transverse(RN, ew)：东西向地球曲率半径, 卯酉圈曲率半径(横着的)
% C_ECEF2ENU: ECEF到ENU转换矩阵
% C_ECEF2NED: ECEF到NED转换矩阵
R0 = 6378137; %WGS84 赤道半径
e = 0.0818191908425; %WGS84 eccentricity
% Calculate meridian radius of curvature using (2.105)
temp = 1 - (e * sin(lat))^2;
Rns = R0 * (1 - e^2) / temp^1.5;
% Calculate transverse radius of curvature using (2.105)
Rew = R0 / sqrt(temp);
clat = cos(lat);
slat = sin(lat);
clon = cos(lon);
slon = sin(lon);
C_ECEF2ENU(1,:) = [-slon , clon, 0];
C_ECEF2ENU(2,:) = [ -slat*clon, -slat*slon clat];
C_ECEF2ENU(3,:) = [ clat*clon, clat*slon, slat];
C_ECEF2NED(1,:) = [-slat*clon, -slat * slon, clat];
C_ECEF2NED(2,:) = [-slon, clon, 0];
C_ECEF2NED(3,:) = [ -clat*clon, -clat*slon, -slat];
end

10
lib/geo/ch_gravity.m Normal file
View File

@ -0,0 +1,10 @@
function gravity = ch_gravity(lat, h)
% gravity: 计算重力(暂时不考虑高度)
%
% INPUT
% lat,纬度(rad)
% h, 高度(m), 暂时没有用
gravity = 9.780325 * ( 1 + 0.0053024*sin(lat)^(2) - 0.0000058*sin(lat*2)^(2));

15
lib/gnss/UTC2GPST.m Normal file
View File

@ -0,0 +1,15 @@
function [gps_week , gps_sec]=UTC2GPST(y, m, d, h, min, s)
% UTC转GPST
if y<90
y=y+2000;
else
y=y+1900;
end
if m<=2
y=y-1;m=m+12;
end
MJD=fix(365.25*y)+fix(30.6001*(m+1))+d+(h+(min+s/60)/60)/24+1720981.5-2400000.5;
gps_week=fix((MJD-44244)/7);
gps_sec=fix(mod((MJD-44244),7))*86400+h*3600+min*60+s;

43
lib/gnss/anheader.m Normal file
View File

@ -0,0 +1,43 @@
function [Obs_types, ant_delta,ifound_types,eof] = anheader(file)
%ANHEADER Analyzes the header of a RINEX file and outputs
% the list of observation types and antenna offset.
% End of file is flagged 1, else 0. Likewise for the types.
% Typical call: anheader('pta.96o')
%Kai Borre 09-12-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $ $Date: 1997/09/23 $
fid = fopen(file,'rt');
eof = 0;
ifound_types = 0;
Obs_types = [];
ant_delta = [];
while 1 % Gobbling the header
line = fgetl(fid);
answer = findstr(line,'END OF HEADER');
if ~isempty(answer), break; end;
if (line == -1), eof = 1; break; end;
answer = findstr(line,'ANTENNA: DELTA H/E/N');
if ~isempty(answer)
for k = 1:3
[delta, line] = strtok(line);
del = str2num(delta);
ant_delta = [ant_delta del];
end;
end
answer = findstr(line,'# / TYPES OF OBSERV');
if ~isempty(answer)
[NObs, line] = strtok(line);
NoObs = str2num(NObs);
for k = 1:NoObs
[ot, line] = strtok(line);
Obs_types = [Obs_types ot];
end;
ifound_types = 1;
end;
end;
%fclose(fid);
%%%%%%%%% end anheader.m %%%%%%%%%

92
lib/gnss/cal2gpstime.m Normal file
View File

@ -0,0 +1,92 @@
%*******************************************************
% DESCRIPTION:
% This function converts calendar date/time
% to GPS week/time.
%
% ARGUMENTS:
% Either 1) a single n x 6 matrix or
% 2) six separate variables that are
% equal dimensioned vectors
%
% Representing:
% year - Two digit calendar year representing the
% range 1980-2079
% (e.g., 99 = 1999 and 01 = 2001).
% month - calendar month, must be in range 1-12.
% day - calendar day, must be in range 1-31.
% hour - hour (UTC), must be in range 0-24.
% min - minutes (UTC), must be in range 0-59.
% sec - seconds (UTC), must be in range 0-59.
%
% [ Note: all arguments must be integers! ]
%
% OUTPUT:
% gps_week - integer GPS week (does not take
% "rollover" into account).
% gps_seconds - integer seconds elapsed in gps_week.
%
% REFERENCE:
% ASEN 5090 class notes (Spring 2003).
%
% MODIFICATIONS:
% 03-14-06 : Jan Weiss - Original/modified
% from old code.
%
% Colorado Center for Astrodynamics Research
% Copyright 2006 University of Colorado, Boulder
%*******************************************************
function [ gps_week, gps_seconds ] = cal2gpstime(varargin)
% Unpack
if nargin == 1
cal_time = varargin{1};
year = cal_time(:,1);
month = cal_time(:,2);
day = cal_time(:,3);
hour = cal_time(:,4);
min = cal_time(:,5);
sec = cal_time(:,6);
clear cal_time
else
year = varargin{1};
month = varargin{2};
day = varargin{3};
hour = varargin{4};
min = varargin{5};
sec = varargin{6};
end
% Seconds in one week
secs_per_week = 604800;
% Converts the two digit year to a four digit year.
% Two digit year represents a year in the range 1980-2079.
if (year >= 80 & year <= 99)
year = 1900 + year;
end
if (year >= 0 & year <= 79)
year = 2000 + year;
end
% Calculates the 'm' term used below from the given calendar month.
if (month <= 2)
y = year - 1;
m = month + 12;
end
if (month > 2)
y = year;
m = month;
end
% Computes the Julian date corresponding to the given calendar date.
JD = floor( (365.25 * y) ) + floor( (30.6001 * (m+1)) ) + ...
day + ( (hour + min / 60 + sec / 3600) / 24 ) + 1720981.5;
% Computes the GPS week corresponding to the given calendar date.
gps_week = floor( (JD - 2444244.5) / 7 );
% Computes the GPS seconds corresponding to the given calendar date.
gps_seconds=round(((((JD-2444244.5)/7)-gps_week)*secs_per_week)/0.5)*0.5;

12
lib/gnss/ch_gpsdop.m Normal file
View File

@ -0,0 +1,12 @@
function [VDOP, HDOP, PDOP, GDOP] = ch_gpsdop(G, lat, lon)
% GPS原理及接收机设计 谢刚 DOP章节
S = [-sin(lon) cos(lon) 0; -sin(lat)*cos(lon) -sin(lat)*sin(lon) cos(lat); cos(lat)*cos(lon) cos(lat)*sin(lon) sin(lat)];
S = [S [0 0 0]'; [0 0 0 1]];
H = (G'*G)^(-1);
H = S*H*S';
VDOP = sqrt(H(3,3));
HDOP = sqrt(H(1,1) + H(2,2));
PDOP = sqrt (H(1,1) + H(2,2) + H(3,3));
GDOP = sqrt(trace(H));
end

43
lib/gnss/ch_gpsls.m Normal file
View File

@ -0,0 +1,43 @@
function [X, dx, G] = ch_gpsls(X, sat_pos, pr)
% GPS 伪距最小二乘法求解， 状态量为 X Y Z B(钟差)
% X: X 值(1:3) 位置delta, (4) 用户钟差偏差
% G: 设计矩阵
% pr: 校正后的伪距
% sat_pos: 卫星位置矩阵
% delta: delta 值(1:3) 位置delta, (4) 用户钟差偏差
B1=1;
END_LOOP=100;
%卫星个数
n = size(sat_pos, 2);
if n < 4
dx = 0;
G = 0;
return
end
for loop = 1:10
% 获得当前位置与各个基站的距离
r = vecnorm(sat_pos - X(1:3));
% 求得H矩阵
H = (sat_pos - X(1:3)) ./ r;
H =-H';
H = [H(:,1:3), ones(n,1)];
dp = ((pr - r) - X(4))';
% 迭代用户距离
dx = (H'*H)^(-1)*H'*dp;
X = X + dx;
G = H;
% END_LOOP = vnorm(delta(1:3));
end
end

78
lib/gnss/ch_sat_pos.m Normal file
View File

@ -0,0 +1,78 @@
function [XYZ, sv_dt]=ch_sat_pos(t, toc, f0, f1, f2, crs, deln, M0, cuc, e, cus, sqrtA, toe, cic, OMG0, cis, i0, crc, omg, OMGd, idot)
% 输入:
% toe: 星历参考时间： 一套星历的有效期为toe前后4小时
% a0 a1 a2 toc: 卫星时钟校正模型方程中3个参数， toc: 第一数据块参考时间, 被用作时钟校正模型中时间参考点
% 卫星星历参数(16个):
% sqrtA: 卫星轨道长半轴的平方根
% es: 轨道偏心率
% i0: toe时刻轨道倾角
% OMEGA0: 周内时等于0时刻的轨道升交点赤经
% omega: 轨道近地角距
% M0: toe时刻的平近点角
% Delta_n： 平均运动角速度校正值
% iDOT： 轨道倾角对时间的变化率
% OMEGA_DOT： 轨道升交点赤经对时间的变化率
% Cuc: 升交点角距余弦和校正振幅
% Cus: 升交点角距正弦和校正振幅
% Cic: 轨道倾角余弦和校正振幅
% Cis: 轨道倾角正弦和校正振幅
%输出:
% X, Y, Z ECEF下卫星位置
% sv_dt： 卫星时钟偏差
%变量:
%摄动改正后的升交距角uk、卫星矢径rk和轨道倾角ik
%卫星在轨道面坐标系中的坐标x,y
%发射时刻升交点的经度L
%以下为计算代码
%1.计算卫星运行的平均角速度
n0=sqrt(3.986005E+14)/(sqrtA.^3);
n=n0+deln;
%2.计算信号发射时卫星的平近点角
sv_dt = f0+f1*(t-toc)+f2*(t-toc).^2;%t为未做钟差改正的观测时刻
t = t - 0;%更新t为做钟差改正后的值
tk = t-toe;%归化时间
if tk>302400
tk=tk-604800;
elseif tk<-302400
tk=tk+604800;
else
tk=tk+0;
end
Mk=M0+n*tk;
%3.计算偏近点角（迭代）
%E=M+e*sin(E);
ed(1)=Mk;
for i=1:3
ed(i+1)=Mk+e*sin(ed(i));
end
Ek=ed(end);
%4.计算真近点角
Vk=atan2(sqrt(1-e.^2)*sin(Ek),(cos(Ek)-e));
%5.计算升交距角（未经改进时）
u=omg+Vk;
%6.计算摄动改正项
deltau=cuc*cos(2*u)+cus*sin(2*u);
deltar=crc*cos(2*u)+crs*sin(2*u);
deltai=cic*cos(2*u)+cis*sin(2*u);
%7.计算摄动改正后的升交距角uk、卫星矢径rk和轨道倾角ik
uk=u+deltau;
rk=(sqrtA.^2)*(1-e*cos(Ek))+deltar;
ik=i0+deltai+idot*tk;
%8.计算卫星在轨道面坐标系中的坐标
x=rk*cos(uk);
y=rk*sin(uk);
%9.计算发射时刻升交点的经度67
L=OMG0+(OMGd-7.2921151467e-5)*tk-7.2921151467e-5*toe;
%10.计算卫星在地固坐标系下坐标
XYZ(1)=x*cos(L)-y*cos(ik)*sin(L);
XYZ(2)=x*sin(L)+y*cos(ik)*cos(L);
XYZ(3)=y*sin(ik);
XYZ = XYZ';
end

8
lib/gnss/ch_sv_pos_rotate.m Normal file
View File

@ -0,0 +1,8 @@
function SP = ch_sv_pos_rotate(SP, tau)
%% 计算经过地球自转改正后的卫星位置
% Earth's rotation rate
% SP: 卫星位置
omega_e = 7.2921151467e-5; %(rad/sec)
theta = omega_e * tau;
SP = [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]*SP;
end

19
lib/gnss/check_rinex_line_length.m Normal file
View File

@ -0,0 +1,19 @@
%*********************************************************************
%
% This function takes a string and checks if it < 80 characters long.
% If so the string is "padded" with zeros until it is 80 characters
% long. Useful when reading RINEX files.
%
%*********************************************************************
function [ current_line ] = check_line_length(current_line)
if length(current_line) < 80
add_spaces = 80 - length(current_line);
for j = 1 : add_spaces
current_line = [ current_line , '0' ];
end
end
% Check if there are any blanks in the data and put a zero there.
current_line = strrep(current_line,' ', '0');

118
lib/gnss/fepoch_0.m Normal file
View File

@ -0,0 +1,118 @@
function [time, dt, sats, eof] = fepoch_0(fid)
% FEPOCH_0 Finds the next epoch in an opened RINEX file with
% identification fid. From the epoch line is produced
% time (in seconds of week), number of sv.s, and a mark
% about end of file. Only observations with epoch flag 0
% are delt with.
%Kai Borre 09-14-96; revised 03-22-97; revised Sept 4, 2001
%Copyright (c) by Kai Borre
%$Revision: 1.0 $ $Date: 1997/09/22 $
%fide = fopen(fid,'rt');
%　该函数只返回ｏ文件中一个历元的以下参数：
% time：周内秒
% dt：接收机钟差（该参数为可选项，不一定所有的o文件中都有）
% sats：当前历元所观测到的卫星
% eof：是否到文件末尾
global sat_index;
time = 0;
dt = 0;
sats = [];
NoSv = 0;
eof = 0;
%循环读取o文件的每一行
while 1
lin = fgets(fid); % earlier fgetl 先读一行
if (feof(fid) == 1); % 若到文件末尾，则结束
eof = 1;
break
end;
% 跳过空行
if length(lin) <= 1
continue;
end
% answer = findstr(lin,'COMMENT'); % 判断该行中是否有字符串“COMMENT”
%
% if ~isempty(answer); % 若有“COMMENT“，则继续读下一行
% lin = fgetl(fid);
% end;
% 如果该行数据第29个字符不为0（0代表改历元正常），
% 或者总长度只有29（也就是没有后面的卫星PRN数据），
% 则结束。
% if ((strcmp(lin(29),'0') == 0) & (size(deblank(lin),2) == 29))
% eof = 1;
% break
% end; % We only want type 0 data
% 如果该行第二个字符是1，或者第29个字符是0，则说明
% 这一行是某一历元的开始行，接下来就可以从该行中提取时间、PRN等参数
if ((strcmp(lin(2),'1') == 1) & (strcmp(lin(29),'0') == 1))
ll = length(lin)-2;
if ll > 60, ll = 60; end;
linp = lin(1:ll);
%fprintf('%60s\n',linp);
%使用strtok函数获得间隔符前面的字符串，
% 首先是获得当前时间
[nian, lin] = strtok(lin);
% year;
[month, lin] = strtok(lin);
% month;
[day, lin] = strtok(lin);
% day;
[hour, lin] = strtok(lin);
% hour
[minute, lin] = strtok(lin);
% minute
[second, lin] = strtok(lin);
% second
[OK_flag, lin] = strtok(lin);
% OK_flag就是第29个字符，如果是0，则该历元正常
%将时间转成数值型，然后再计算出GPS周和周内秒
h = str2num(hour)+str2num(minute)/60+str2num(second)/3600;
jd = julday(str2num(nian)+2000, str2num(month), str2num(day), h);
[week, sec_of_week] = gps_time(jd);
time = sec_of_week;
%获得该历元卫星数
[NoSv, lin] = strtok(lin,'G');
%储存该历元每颗卫星的PRN
for k = 1:str2num(NoSv)
[sat, lin] = strtok(lin,'G');
prn(k) = str2num(sat);
end
% prn是1行NoSv列的矩阵，sats是其转置
sats = prn(:);
% 接收及钟差，有的o文件中没有该参数
dT = strtok(lin);
if isempty(dT) == 0 %如果dT不为0，则将其记录
dt = str2num(dT);
end
break % 跳出while循环
end
end;
% datee=[str2num(nian) str2num(month) str2num(day) str2num(hour) str2num(minute) str2num(second)];
%%%%%%%% end fepoch_0.m %%%%%%%%%

16
lib/gnss/get_eph.m Normal file
View File

@ -0,0 +1,16 @@
function eph = get_eph(ephemeridesfile)
%GET_EPH The ephemerides contained in ephemeridesfile
% are reshaped into a matrix with 21 rows and
% as many columns as there are ephemerides.
% Typical call eph = get_eph('rinex_n.dat')
%Kai Borre 10-10-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $ $Date: 1997/09/26 $
fide = fopen(ephemeridesfile);
[eph, count] = fread(fide, Inf, 'double');
noeph = count/23;
eph = reshape(eph, 23, noeph); % ephΪһ¸ö23ÐÐnoephÁеľØÕó
%%%%%%%% end get_eph.m %%%%%%%%%%%%%%%%%%%%%

105
lib/gnss/iono_correction.m Normal file
View File

@ -0,0 +1,105 @@
%This Function approximate Ionospheric Group Delay 这个函数近似电离层群延迟
%CopyRight By Moein Mehrtash
%************************************************************************
% Written by Moein Mehrtash, Concordia University, 3/21/2008 *
% Email: moeinmehrtash@yahoo.com *
%************************************************************************
% ***********************************************************************
% Function for computing an Ionospheric range correction for the *
% GPS L1 frequency from the parameters broadcasted in the GPS *
% Navigation Message.功能:根据GPS导航信息中广播的参数计算电离层距离修正的GPS L1频率。 *
% ==================================================================
% References: *
% Klobuchar, J.A., (1996) "Ionosphercic Effects on GPS", in *
% Parkinson, Spilker (ed), "Global Positioning System Theory and *
% Applications, pp.513-514. *
% ICD-GPS-200, Rev. C, (1997), pp. 125-128 *
% NATO, (1991), "Technical Characteristics of the NAVSTAR GPS", *
% pp. A-6-31 - A-6-33 *
% ==================================================================
% Input : *
% Pos_Rcv : XYZ position of reciever (Meter) *
% Pos_SV : XYZ matrix position of GPS satellites (Meter) *
% GPS_Time : Time of Week (sec) *
% Alfa(4) : The coefficients of a cubic equation *
% representing the amplitude of the vertical *
% dalay (4 coefficients - 8 bits each) 表示垂直dalay振幅的三次方程的系数(4个系数，每个8位) *
% Beta(4) : The coefficients of a cubic equation *
% representing the period of the model *
% (4 coefficients - 8 bits each) 表示模型周期的三次方程的系数(4个系数-各8位) *
% Output: *
% Delta_I : Ionospheric slant range correction for *
% the L1 frequency电离层L1频率的倾斜距离校正(Sec) *
% ==================================================================
function [ delay ]=iono_correction(RP, SP, alpha, beta, gpst)
% RP: 接收机位置
% SP: 卫星位置
% Alpha Beta 电离层校准参数
% GPS Time
% delay 单位为m
if norm(RP) < 1
delay = 0;
return;
end
[lat, lon, ~] = ch_ECEF2LLA(RP);
lat = lat/pi;
lon =lon/pi; % semicircles unit Lattitdue and Longitude
[el, az] = satellite_az_el(SP, RP);
E = az/pi; %SemiCircle Elevation
A = el; %SemiCircle Azimoth
% Calculate the Earth-Centered angle, Psi
Psi = 0.0137/(E+.11)-0.022; %SemiCircle
%Compute the Subionospheric lattitude, Phi_L
Phi_L = lat+Psi*cos(A); %SemiCircle
if Phi_L>0.416
Phi_L=0.416;
elseif Phi_L<-0.416
Phi_L=-0.416;
end
%Compute the subionospheric longitude, Lambda_L
Lambda_L = lon+(Psi * sin(A)/cos(Phi_L*pi)); %SemiCircle
%Find the geomagnetic lattitude ,Phi_m, of the subionospheric location
%looking toward each GPS satellite:
Phi_m = Phi_L+0.064*cos((Lambda_L-1.617)*pi);
%Find the Local Time ,t, at the subionospheric point
t=4.32*10^4*Lambda_L+gpst; %GPS_Time(Sec)
t= mod(t,86400);
if t>86400
t = t-86400;
elseif t<0
t=t+86400;
end
%Convert Slant time delay, Compute the Slant Factor,F
F=1+16*(.53-E)^3;
%Compute the ionospheric time delay T_iono by first computing x
Per=beta(1)+beta(2)*Phi_m+beta(3)*Phi_m^2+beta(4)*Phi_m^3;
if Per <72000 %Period
Per=72000;
end
x=2*pi*(t-50400)/Per; %Rad
AMP=alpha(1)+alpha(2)*Phi_m+alpha(3)*Phi_m^2+alpha(4)*Phi_m^3;
if AMP<0
AMP=0;
end
if abs(x)>1.57
T_iono=F*5*10^(-9);
else
T_iono=F*(5*10^(-9)+AMP*(1-x^2/2+x^4/24));
end
delay = T_iono*299792458;

91
lib/gnss/parsef.m Normal file
View File

@ -0,0 +1,91 @@
function varargout = parsef(input, format)
%parsef parse string value using FORTRAN formatting codes
% [val1,val2,...valn] = parsef(input, format)
% input is string input value
% format is cell array of format codes
global input_
input_ = input;
varargout = getvals(1, format, 1);
clear global input_
return
% this function does the work. you probably don't want to go here
function [output, idx] = getvals(idx, format, reps)
global input_
count = 1;
output = {};
for k = 1:reps
odx = 1;
for i = 1:length(format)
fmt = format{i};
switch class(fmt)
case 'double'
count = fmt;
case 'char'
type = fmt(1);
if type == 'X'
idx = idx+count;
else
[len,cnt] = sscanf(fmt,'%*c%d',1);
if cnt ~= 1
error(['Invalid format specifier: ''',fmt,'''']);
end
switch type
case {'I','i'}
for j = 1:count
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%d',1);
if cnt == 1
output{odx}(j,k) = val;
else
output{odx}(j,k) = NaN;
end
idx = idx+len;
end
case {'F','f'}
for j = 1:count
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%f',1);
if cnt == 1
output{odx}(j,k) = val;
else
output{odx}(j,k) = NaN;
end
idx = idx+len;
end
case {'E','D','G'}
for j = 1:count
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%f%*1[DdEe]%f',2);
if cnt == 2
output{odx}(j,k) = val(1) * 10^val(2); %#ok<AGROW>
elseif cnt == 1
output{odx}(j,k) = val;
else
output{odx}(j,k) = NaN;
end
idx = idx+len;
end
case 'A'
for j = 1:count
output{odx}{j,k} = input_(idx:min(idx+len-1,end));
idx = idx+len;
end
otherwise
error(['Invalid format specifier: ''',fmt,'''']);
end
odx = odx+1;
end
count = 1;
case 'cell'
[val, idx] = getvals(idx, fmt, count);
if length(val) == 1
output(odx) = val;
else
output{odx} = val;
end
odx = odx+1;
count = 1;
end
end
end
return

95
lib/gnss/read_rinex_nav.m Normal file
View File

@ -0,0 +1,95 @@
% Lucas Ward
% ASEN 5090
% Read the RINEX navigation file. The header is skipped because
% information in it (a0, a1, iono alpha and beta parameters) is not
% currently needed for orbit computation. This can be easily amended to
% include navigation header information by adding lines in the 'while' loop
% where the header is currently skipped.
%
% Input: - filename - enter the filename to be read. If filename
% exists, the orbit will be calculated.
%
% Output: - ephemeris - Output is a matrix with rows for each PRN and
% columns as follows:
%
% col 1: PRN ....... satellite PRN
% col 2: M0 ....... mean anomaly at reference time
% col 3: delta_n ..... mean motion difference
% col 4: e ....... eccentricity
% col 5: sqrt(A) ..... where A is semimajor axis
% col 6: OMEGA ....... LoAN at weekly epoch
% col 7: i0 ....... inclination at reference time
% col 8: omega ....... argument of perigee
% col 9: OMEGA_dot ... rate of right ascension
% col 10: i_dot ....... rate of inclination angle
% col 11: Cuc ....... cosine term, arg. of latitude
% col 12: Cus ....... sine term, arg. of latitude
% col 13: Crc ....... cosine term, radius
% col 14: Crs ....... sine term, radius
% col 15: Cic ....... cosine term, inclination
% col 16: Cis ....... sine term, inclination
% col 17: toe ....... time of ephemeris
% col 18: IODE ....... Issue of Data Ephemeris
% col 19: GPS_wk ....... GPS week
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ephemeris = read_rinex_nav( filename )
fid = fopen(filename);
if fid == -1
errordlg(['The file ''' filename ''' does not exist.']);
return;
end
% skip through header
end_of_header = 0;
while end_of_header == 0
current_line = fgetl(fid);
if strfind(current_line,'END OF HEADER')
end_of_header=1;
end
end
j = 0;
while feof(fid) ~= 1
j = j+1;
current_line = fgetl(fid);
% parse epoch line (ignores SV clock bias, drift, and drift rate)
[PRN, Y, M, D, H, min, sec,af0,af1,af2] = parsef(current_line, {'I2' 'I3' 'I3' 'I3' 'I3' 'I3' ...
'F5.1','D19.12','D19.12','D19.12'});
% Broadcast orbit line 1
current_line = fgetl(fid);
[IODE Crs delta_n M0] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 2
current_line = fgetl(fid);
[Cuc e Cus sqrtA] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 3
current_line = fgetl(fid);
[toe Cic OMEGA Cis] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 4
current_line = fgetl(fid);
[i0 Crc omega OMEGA_dot] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 5
current_line = fgetl(fid);
[i_dot L2_codes GPS_wk L2_dataflag ] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 6
current_line = fgetl(fid);
[SV_acc SV_health TGD IODC] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12' 'D19.12' 'D19.12'});
% Broadcast orbit line 7
current_line = fgetl(fid);
[msg_trans_t fit_int ] = parsef(current_line, {'D22.12' 'D19.12' 'D19.12'});
[ gps_week, toc ] = UTC2GPST(Y, M, D, H, min, sec);
ephemeris(j,:) = [PRN, M0, delta_n, e, sqrtA, OMEGA, i0, omega, OMEGA_dot, i_dot, Cuc, Cus, Crc, Crs, Cic, Cis, toe, IODE, GPS_wk,toc,af0,af1,af2,TGD];
end

216
lib/gnss/read_rinex_obs.m Normal file
View File

@ -0,0 +1,216 @@
function [rinex,rec_xyz] = read_rinex_obs(fname,nlines)
%*******************************************************
% function [ rinex ] = read_rinex_obs(fname)
%
% DESCRIPTION:
% This function reads a RINEX format GPS data
% file and returns the data in an array.
%
% ARGUMENTS:
% fname (str) - RINEX file
%
% OUTPUT:
% rinex - rinex data (v3 struct)
%
% CALLED BY:
% process_rinex.m
%
% FUNCTIONS CALLED:
% read_rinex_header.m
%
% MODIFICATIONS:
% XX-XX-03 : Jan Weiss - Original
% 03-14-06 : Jan Weiss - Cleanup
% : See SVN log for further modifications
% 10-26-06 : simplified for class positioning project & allows for
% limited number of lines read (PA)
%
% Colorado Center for Astrodynamics Research
% Copyright 2006 University of Colorado, Boulder
%*******************************************************
if (nargin < 2)
nlines = 1e6;
end
% Initialize variables
rinex_data = [];
line_count = 1;
% Read header
[ fid, rec_xyz, observables ] = read_rinex_header(fname);
num_obs = length(observables);
% Get the first line of the observations.
current_line = fgetl(fid);
% If not at the end of the file, search for the desired information.
while current_line ~= -1 & line_count < nlines
% Get the time for this data epoch.
current_time = [ str2num(current_line(2:3)) ; str2num(current_line(5:6)) ; ...
str2num(current_line(8:9)) ; str2num(current_line(11:12)) ; ...
str2num(current_line(14:15)) ; str2num(current_line(17:27)) ]';
% How many SV's are there?
current_num_sv = str2num(current_line(30:32));
% Figure out which PRN's there are.
for ii=1:current_num_sv
current_prn(ii) = str2num(current_line(31 + 3*ii : 32 + 3*ii));
end
% Get the data for all SV's in this epoch.
for ii=1:current_num_sv
% Get the next line.
current_line = fgetl(fid);
line_count = line_count + 1;
% Check the length of the line and pad it with zeros to
% make sure it is 80 characters long.
current_line = check_rinex_line_length(current_line);
% Get the observables on this line.
current_obs = [ str2num(current_line(1:14)) ; str2num(current_line(17:30)) ; ...
str2num(current_line(33:46)) ; str2num(current_line(49:62)) ; str2num(current_line(65:78)) ];
% If there are > 5 observables, read another line to get the rest of the observables for this SV.
if num_obs > 5
% Get the next line.
current_line = fgetl(fid);
line_count = line_count + 1;
% Check the length of the line and pad it with zeros to
% make sure it is 80 characters long.
current_line = check_rinex_line_length(current_line);
% Append the data in this line to the data from previous line.
current_obs = [ current_obs ; str2num(current_line(1:14)) ; ...
str2num(current_line(17:30)) ; str2num(current_line(33:46)) ; ...
str2num(current_line(49:62)) ; str2num(current_line(65:78)) ];
end % if num_obs > 5
% Format the data for this PRN as Date/Time, PRN, Observations.
current_data = [ current_time , current_prn(ii) , current_obs' ];
% Keep only data for the specified PRNs
if nargin == 3 & PRN_list & isempty(find(PRN_list == current_prn(ii)))
continue
end
%Append to the master rinex data file.
rinex_data = [ rinex_data ; current_data ];
end % for ii=1:current_num_sv
% Get the next line.
current_line = fgetl(fid);
line_count = line_count + 1;
end % while current_line ~= -1
% Convert time format
[ gpswk, gpssec ] = cal2gpstime(rinex_data(:,1:6));
rinex.data = [ gpswk gpssec rinex_data(:, 7:end) ];
% Define columns
rinex = define_cols(rinex, observables);
% Convert CP to meters
rinex = convert_rinex_CP(rinex);
% =========================================================================
function rinex = define_cols(rinex, observables)
% Defaults
rinex.col.WEEK = 1;
rinex.col.TOW = 2;
rinex.col.PRN = 3;
col_offset = 3;
for ii=1:length(observables)
switch observables{ii}
case {'L1'}
rinex.col.L1 = ii + col_offset;
case {'L2'}
rinex.col.L2 = ii + col_offset;
case {'P1'}
rinex.col.P1 = ii + col_offset;
case {'P2'}
rinex.col.P2 = ii + col_offset;
case {'C1'}
rinex.col.C1 = ii + col_offset;
case {'D1'}
rinex.col.D1 = ii + col_offset;
case {'S1'}
rinex.col.S1 = ii + col_offset;
case {'S2'}
rinex.col.S2 = ii + col_offset;
end % switch
end % for ii=1:length(observables)
% =========================================================================
function [ rinex ] = convert_rinex_CP(rinex)
set_constants;
if rinex.col.L1 ~= 0
rinex.data(:, rinex.col.L1) = rinex.data(:, rinex.col.L1) * LAMBDA_L1;
end
% =========================================================================
function [ fid, rec_xyz, observables ] = read_rinex_header( file_name )
% Initialize the observables variable.
observables={};
% Assign a file ID and open the given header file.
fid=fopen(file_name);
% If the file does not exist, scream bloody murder!
if fid == -1
display('Error! Header file does not exist.');
else
% Set up a flag for when the header file is done.
end_of_header=0;
% Get the first line of the file.
current_line = fgetl(fid);
% If not at the end of the file, search for the desired information.
while end_of_header ~= 1
% Search for the approximate receiver location line.
if strfind(current_line,'APPROX POSITION XYZ')
% Read xyz coordinates into a matrix.
[rec_xyz] = sscanf(current_line,'%f');
end
% Search for the number/types of observables line.
if strfind(current_line,'# / TYPES OF OBSERV')
% Read the non-white space characters into a temp variable.
[observables_temp] = sscanf(current_line,'%s');
% Read the number of observables space and then create
% a matrix containing the actual observables.
for ii = 1:str2num(observables_temp(1))
observables{ii} = observables_temp( 2*ii : 2*ii+1 );
end
end
% Get the next line of the header file.
current_line = fgetl(fid);
%Check if this line is at the end of the header file.
if strfind(current_line,'END OF HEADER')
end_of_header=1;
end
end
end

204
lib/gnss/rinexe.m Normal file
View File

@ -0,0 +1,204 @@
function iono = rinexe(ephemerisfile, outputfile)
%RINEXE Reads a RINEX Navigation Message file and
% reformats the data into a matrix with 23s
% rows and a column for each satellite.
% The matrix is stored in outputfile
%Typical call: rinexe('pta.96n','pta.nav')
%Kai Borre 04-18-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $ $Date: 1997/09/24 $
% Units are either seconds, meters, or radians
fide = fopen(ephemerisfile);
head_lines = 0;
iono = zeros(8,1);
%read the header
header_end = [];
while (isempty(header_end))
head_lines = head_lines+1;
%read the line and search the ionosphere labels
lin = fgetl(fide);
vers_found = ~isempty(strfind(lin,'RINEX VERSION / TYPE'));
iono_found = (~isempty(strfind(lin,'ION ALPHA')) || ~isempty(strfind(lin,'IONOSPHERIC CORR')));
%if the ionosphere parameters label was found
if (vers_found)
version = str2num(lin(1:9));
end
%if the ionosphere parameters label was found
if (iono_found)
%change flag
% ioparam = 1;
%save the 8 ionosphere parameters
data = textscan(lin(5:end),'%f%f%f%f%*[^\n]');
if ~isempty(data(4))
iono(1) = data{1};
iono(2) = data{2};
iono(3) = data{3};
iono(4) = data{4};
lin = [];
while isempty(lin)
lin = fgetl(fide);
end
data = textscan(lin(5:end),'%f%f%f%f%*[^\n]');
if ~isempty(data(4))
iono(5) = data{1};
iono(6) = data{2};
iono(7) = data{3};
iono(8) = data{4};
else
iono = zeros(8,1);
end
end
end
header_end = strfind(lin,'END OF HEADER');
end
head_lines = head_lines + 1;
noeph = -1; %初始化星历数目
while 1
noeph = noeph+1;
line = fgetl(fide);
if line == -1, break; end
end;
noeph = noeph/8 % 卫星星历数 = 去掉文件头之后的文件行数 / 8
% 每颗卫星星历行数是8行
% 下面两行作用是回到文件头结束的地方
frewind(fide);
for i = 1:head_lines, line = fgetl(fide); end;
% Set aside memory for the input，
% 为星历中每个参数产生1行noeph列的0矩阵，
% 每一列对应该星历文件中的一个PRN
noeph = fix(noeph);
svprn = zeros(1,noeph);
weekno = zeros(1,noeph);
t0c = zeros(1,noeph);
tgd = zeros(1,noeph);
aodc = zeros(1,noeph);
toe = zeros(1,noeph);
af2 = zeros(1,noeph);
af1 = zeros(1,noeph);
af0 = zeros(1,noeph);
aode = zeros(1,noeph);
deltan = zeros(1,noeph);
M0 = zeros(1,noeph);
ecc = zeros(1,noeph);
roota = zeros(1,noeph);
toe = zeros(1,noeph);
cic = zeros(1,noeph);
crc = zeros(1,noeph);
cis = zeros(1,noeph);
crs = zeros(1,noeph);
cuc = zeros(1,noeph);
cus = zeros(1,noeph);
Omega0 = zeros(1,noeph);
omega = zeros(1,noeph);
i0 = zeros(1,noeph);
Omegadot = zeros(1,noeph);
idot = zeros(1,noeph);
accuracy = zeros(1,noeph);
health = zeros(1,noeph);
fit_interval = zeros(1,noeph);
% 将所有卫星的星历参数存入上面定义的矩阵中
for i = 1:noeph
line = fgetl(fide); % 从该组星历中第一颗卫星的星历的第一行开始
svprn(i) = str2num(line(1:2)); % 存入卫星PRN
year = line(3:6);
month = line(7:9);
day = line(10:12);
hour = line(13:15);
minute = line(16:18);
second = line(19:22);
af0(i) = str2num(line(23:41));
af1(i) = str2num(line(42:60));
af2(i) = str2num(line(61:79));
line = fgetl(fide); % 读下一行
IODE = line(4:22);
crs(i) = str2num(line(23:41));
deltan(i) = str2num(line(42:60));
M0(i) = str2num(line(61:79));
line = fgetl(fide); % 读下一行
cuc(i) = str2num(line(4:22));
ecc(i) = str2num(line(23:41));
cus(i) = str2num(line(42:60));
roota(i) = str2num(line(61:79));
line=fgetl(fide); % 读下一行
toe(i) = str2num(line(4:22));
cic(i) = str2num(line(23:41));
Omega0(i) = str2num(line(42:60));
cis(i) = str2num(line(61:79));
line = fgetl(fide); % 读下一行
i0(i) = str2num(line(4:22));
crc(i) = str2num(line(23:41));
omega(i) = str2num(line(42:60));
Omegadot(i) = str2num(line(61:79));
line = fgetl(fide); % 读下一行
idot(i) = str2num(line(4:22));
codes = str2num(line(23:41));
weekno = str2num(line(42:60));
L2flag = str2num(line(61:79));
line = fgetl(fide); % 读下一行
svaccur = str2num(line(4:22));
svhealth(i) = str2num(line(23:41));
tgd(i) = str2num(line(42:60));
iodc = line(61:79);
line = fgetl(fide); % 读下一行
if length(line)>= 41
tom(i) = str2num(line(4:22));
fit_interval(i) = str2num(line(23:41));
else
tom(i) = str2num(line(4:22));
fit_interval(i) = 0;
end
% spare = line(42:60);
% spare = line(61:79);
end
status = fclose(fide)
% Description of variable eph.
% 把上面存好的各参数再转存到一个大矩阵“eph”中，该矩阵每行代表一种参数
% 每列代表一颗卫星
eph(1,:) = svprn;
eph(2,:) = af2;
eph(3,:) = M0;
eph(4,:) = roota;
eph(5,:) = deltan;
eph(6,:) = ecc;
eph(7,:) = omega;
eph(8,:) = cuc;
eph(9,:) = cus;
eph(10,:) = crc;
eph(11,:) = crs;
eph(12,:) = i0;
eph(13,:) = idot;
eph(14,:) = cic;
eph(15,:) = cis;
eph(16,:) = Omega0;
eph(17,:) = Omegadot;
eph(18,:) = toe;
eph(19,:) = af0;
eph(20,:) = af1;
eph(21,:) = toe;
eph(22,:) = fit_interval;
eph(23,:) = svhealth;
fidu = fopen(outputfile,'w');
count = fwrite(fidu,[eph],'double');
fclose all
%%%%%%%%% end rinexe.m %%%%%%%%%


25
lib/gnss/satellite_az_el.m Normal file
View File

@ -0,0 +1,25 @@
function [az, el] = satellite_az_el(SP, RP)
%% 计算卫星俯仰角(el), 和方位角(az)
% get_satellite_az_el: computes the satellite azimuth and elevation
% given the position of the user and the satellite in ECEF
% : SP: 卫星位置ECEF
% RP: 用户位置ECEF
% Output Args:
% az: azimuth: 方位角
% el: elevation： 俯仰角
% 参考 GPS 原理及接收机设计 谢刚
[lat, lon, ~] = ch_ECEF2LLA(RP);
C_ECEF2ENU = [-sin(lon) cos(lon) 0; -sin(lat)*cos(lon) -sin(lat)*sin(lon) cos(lat); cos(lat)*cos(lon) cos(lat)*sin(lon) sin(lat)];
enu = C_ECEF2ENU*[SP - RP];
% nroamlized line of silght vector
enu = enu / norm(enu);
az = atan2(enu(1), enu(2));
el = asin(enu(3)/norm(enu));
end

92
lib/gnss/set_constants.m Normal file
View File

@ -0,0 +1,92 @@
%*******************************************************
%
% DESCRIPTION:
% This script contains many useful constants for GPS
% and related work. It should be kept in only
% one place so that updates are immediately available
% to all other scripts/functions.
%
% ARGUMENTS:
% None, just call this script to place
% the constants in your workspace.
%
% OUTPUT:
% Variables in your current workspace.
%
% CALLED BY:
% Many other codes.
%
% FUNCTIONS CALLED:
% None.
%
% MODIFICATIONS:
% XX-XX-02 : Jan Weiss - Original
% 07-25-04 : Jan Weiss - updated header.
% 10-19-04 : Jan Weiss - Cleanup and added
% some conversion factors.
% : See SVN log for further updates.
%
% Colorado Center for Astrodynamics Research
% Copyright 2005 University of Colorado, Boulder
%*******************************************************
% GENERAL CONSTANTS
% =========================================================================
c = 299792458; %----> Speed of light (meters/s).
Re = 6378137 ; %----> Earth Radius (meters)
% =========================================================================
% CONVERSION FACTORS
% =========================================================================
Hz2MHz = 1E-6;
MHz2Hz = 1E6;
s2ns = 1E9;
ns2s = 1E-9;
s2micros = 1E6;
micros2s = 1E-6;
s2ms = 1E3;
ms2s = 1E-3;
dtr = pi / 180;
rtd = 180 / pi;
m2cm = 100;
cm2m = 1 / 100;
m2mm = 1000;
mm2m = 1 / 1000;
ft2m = 0.3048; % Source: http://www.nodc.noaa.gov/dsdt/ucg/
m2ft = 1 / 0.3048;
ns2m = c * ns2s; % Converts time in nano-sec to distance,
% assuming the speed of light.
% =========================================================================
% GNSS SPECIFIC CONSTANTS
% =========================================================================
L1 = 1575.42e6; %----> Freqs in Hz.
L2 = 1227.60e6;
L5 = 1176.45e6;
L1MHz = 1575.42; %----> Freqs in MHz.
L2MHz = 1227.60;
L5MHz = 1176.45;
L1GHz = 1.57542; %----> Freqs in GHz.
L2GHz = 1.22760;
L5GHz = 1.17645;
LAMBDA_L1 = c / L1; %----> Wavelengths in meters.
LAMBDA_L2 = c / L2;
LAMBDA_L5 = c / L5;
CA_CODE_RATE = 1.023e6; %----> C/A and P code chipping rate in chips/s.
P_CODE_RATE = 10.23e6;
CA_CHIP_PERIOD = 1 / CA_CODE_RATE; %----> C/A & P code chip periods in s.
P_CHIP_PERIOD = 1 / P_CODE_RATE;
CA_CHIP_LENGTH = c / CA_CODE_RATE; %----> C/A & P code chip lengths in meters.
P_CHIP_LENGTH = c / P_CODE_RATE;
CA_CODE_LENGTH = 1023; % chips
% =========================================================================

37
lib/gnss/sv_clock_bias.m Normal file
View File

@ -0,0 +1,37 @@
function dtr = sv_clock_bias(t, toc, a0, a1, a2, e, sqrtA, toe, Delta_n, M0)
% 输入:
% a0 a1 a2 toc: 卫星时钟校正模型方程中3个参数， toc: 第一数据块参考时间, 被用作时钟校正模型中时间参考点
% dtr： 卫星时钟偏差
dtr = a0+a1*(t-toc)+a2*(t-toc).^2;%t为未做钟差改正的观测时刻
F = -4.442807633e-10;
mu = 3.986005e14;
A = sqrtA^2;
cmm = sqrt(mu/A^3); % computed mean motion
tk = t - toe;
% account for beginning or end of week crossover
if (tk > 302400)
tk = tk-604800;
end
if (tk < -302400)
tk = tk+604800;
end
% apply mean motion correction
n = cmm + Delta_n;
% Mean anomaly
mk = M0 + n*tk;
% solve for eccentric anomaly
Ek = mk;
Ek = mk + e*sin(Ek);
Ek = mk + e*sin(Ek);
Ek = mk + e*sin(Ek);
% dsv 时间为s
dtr = dtr + F*e*sqrtA*sin(Ek);
end

86
lib/gnss/tropo_correction.m Normal file
View File

@ -0,0 +1,86 @@
function [corr] = tropo_correction(el, h)
% SYNTAX:
% [corr] = tropo_error_correction(el, h);
%
% INPUT:
% el = satellite elevation (rad)
% h = receiver ellipsoidal height (m)
%
% OUTPUT:
% corr = tropospheric error correction
%
% DESCRIPTION:
% Computation of the pseudorange correction due to tropospheric refraction.
% Saastamoinen algorithm.
%----------------------------------------------------------------------------------------------
% goGPS v0.4.3
%
% Copyright (C) 2009-2014 Mirko Reguzzoni, Eugenio Realini
%
% Portions of code contributed by Laboratorio di Geomatica, Polo Regionale di Como,
% Politecnico di Milano, Italy
%----------------------------------------------------------------------------------------------
global tropo_model
%Saastamoinen model requires positive ellipsoidal height
h(h < 0) = 0;
% If in ATM_model section in config file tropo is set to 0 it does not use tropospheric model
% [ATM_model]
% tropo=0
if (tropo_model == 0)
corr = zeros(size(el));
else
if (h < 5000)
%conversion to radians
el = abs(el);
%Standard atmosphere - Berg, 1948 (Bernese)
%pressure [mbar]
Pr = 1013.25;
%temperature [K]
Tr = 291.15;
%numerical constants for the algorithm [-] [m] [mbar]
Hr = 50.0;
P = Pr * (1-0.0000226*h).^5.225;
T = Tr - 0.0065*h;
H = Hr * exp(-0.0006396*h);
%----------------------------------------------------------------------
%linear interpolation
h_a = [0; 500; 1000; 1500; 2000; 2500; 3000; 4000; 5000];
B_a = [1.156; 1.079; 1.006; 0.938; 0.874; 0.813; 0.757; 0.654; 0.563];
t = zeros(length(T),1);
B = zeros(length(T),1);
for i = 1 : length(T)
d = h_a - h(i);
[dmin, j] = min(abs(d));
if (d(j) > 0)
index = [j-1; j];
else
index = [j; j+1];
end
t(i) = (h(i) - h_a(index(1))) ./ (h_a(index(2)) - h_a(index(1)));
B(i) = (1-t(i))*B_a(index(1)) + t(i)*B_a(index(2));
end
%----------------------------------------------------------------------
e = 0.01 * H .* exp(-37.2465 + 0.213166*T - 0.000256908*T.^2);
%tropospheric error
corr = ((0.002277 ./ sin(el)) .* (P - (B ./ (tan(el)).^2)) + (0.002277 ./ sin(el)) .* (1255./T + 0.05) .* e);
else
corr = zeros(size(el));
end
end

73
lib/madgwick.m Normal file
View File

@ -0,0 +1,73 @@
classdef madgwick < handle
methods (Static = true)
function q = imu(q, Gyroscope, Accelerometer, SamplePeriod, Beta)
% Normalise accelerometer measurement
acc = Accelerometer;
if(norm(acc) == 0), return; end % handle NaN
acc = acc / norm(acc); % normalise magnitude
% Gradient decent algorithm corrective step
F = (quatrotate(q, [0 0 1]) - acc)';
% F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
% 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
% 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];
J = [-2*q(3), 2*q(4), -2*q(1), 2*q(2)
2*q(2), 2*q(1), 2*q(4), 2*q(3)
0, -4*q(2), -4*q(3), 0 ];
step = (J'*F);
step = step / norm(step); % normalise step magnitude
% Compute rate of change of quaternion
qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';
% Integrate to yield quaternion
q = q + qd * SamplePeriod;
q = q / norm(q); % normalise quaternion
end
function q = ahrs(q, Gyroscope, Accelerometer, Magnetometer, SamplePeriod, Beta)
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0), return; end % handle NaN
acc = Accelerometer / norm(Accelerometer); % normalise magnitude
% Normalise magnetometer measurement
if(norm(Magnetometer) == 0), return; end % handle NaN
mag = Magnetometer / norm(Magnetometer); % normalise magnitude
% Reference direction of Earth's magnetic feild
h = quatrotate(quatconj(q), mag);
h = [0 h];
b = [0 norm([h(2) h(3)]) 0 h(4)];
% Gradient decent algorithm corrective step
F= (quatrotate(q, [0 0 1]) - acc)';
F = [F; (quatrotate(q, [b(2) 0 b(4)]) - mag)'];
% F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
% 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
% 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
% 2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - Magnetometer(1)
% 2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - Magnetometer(2)
% 2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - Magnetometer(3)];
J = [-2*q(3), 2*q(4), -2*q(1), 2*q(2)
2*q(2), 2*q(1), 2*q(4), 2*q(3)
0, -4*q(2), -4*q(3), 0
-2*b(4)*q(3), 2*b(4)*q(4), -4*b(2)*q(3)-2*b(4)*q(1), -4*b(2)*q(4)+2*b(4)*q(2)
-2*b(2)*q(4)+2*b(4)*q(2), 2*b(2)*q(3)+2*b(4)*q(1), 2*b(2)*q(2)+2*b(4)*q(4), -2*b(2)*q(1)+2*b(4)*q(3)
2*b(2)*q(3), 2*b(2)*q(4)-4*b(4)*q(2), 2*b(2)*q(1)-4*b(4)*q(3), 2*b(2)*q(2)];
step = (J'*F);
step = step / norm(step); % normalise step magnitude
% Compute rate of change of quaternion
qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';
% Integrate to yield quaternion
q = q + qd * SamplePeriod;
q = q / norm(q); % normalise quaternion
end
end
end

52
lib/mahony.m Normal file
View File

@ -0,0 +1,52 @@
classdef mahony < handle
methods (Static = true)
function q = imu(q, gyr, acc, dt, Kp)
% 속醵똑데貫뺏
norm_acc = norm(acc);
if((norm_acc) == 0), return; end
acc = acc / norm(acc);
qtmp = att_upt(q, gyr, dt);
v = qmulv(qconj(qtmp), [0 0 1]');
e = cross(acc, v) ;
% 럽웩
gyr = gyr + Kp * e ;
% 생롸
q = qintg(q, gyr, dt);
end
function q = ahrs(q, gyr, acc, mag, dt, Kp)
% 속醵똑셕데貫뺏
if(norm(acc) == 0), return; end % handle NaN
acc = acc / norm(acc); % normalise magnitude
% 늚끝데貫뺏
if(norm(mag) == 0), return; end % handle NaN
mag = mag / norm(mag); % normalise magnitude
% Reference direction of Earth's magnetic feild
h = qmulv(q, mag);
b = [norm([h(1) h(2)]) 0 h(3)]';
% Estimated direction of gravity and magnetic field
w = qmulv(qconj(q), b);
v = qmulv(qconj(q), [0 0 1]');
% Error is sum of cross product between estimated direction and measured direction of fields
e = cross(acc, v) + cross(mag, w);
% Apply feedback terms
gyr = gyr + Kp * e ;
% integate
q = qintg(q, gyr, dt);
end
end
end

23
lib/mat2c.m Normal file
View File

@ -0,0 +1,23 @@
function mat2c(mat)
[r, c] = size(mat);
mn = r*c;
fprintf('{');
if r==1||c==1
for i=1:mn-1
fprintf('%f,', mat(i));
end
fprintf('%f};\n', mat(i+1));
else
for i=1:r-1
for j=1:c
fprintf('%f,', mat(i,j));
end
fprintf('\n');
end
for j=1:c-1
fprintf('%f,', mat(i+1,j));
end
fprintf('%f};\n', mat(i+1,j+1));
end
end

81
lib/multilateration.m Normal file
View File

@ -0,0 +1,81 @@
function pos = multilateration(sv_pos, pos, pr, dim)
%% 最小二乘法多边测距 解算标签位置
% M x N: M:维度 2 OR 3 N:基站个数
% sv_pos: 基站位置 M x N
% pos: M x 1
% pr: 伪距 N x 1
% dim : 2 or 3 : 2: 二维定位 3: 三维定位
B1 = 0.1; % 迭代收敛阈值（位置更新量小于此值时停止）
END_LOOP = 100; % 初始迭代残差（设为较大值）
sv_num = size(sv_pos, 2); % 基站数量
max_retry = 5; % 最大迭代次数
lpos = pos; % 保存上一次的位置，用于迭代失败时回退
% 设置约束，如果基站数量少于3，则直接退出。
% 因为解算位置需满足以下条件：
% 2D定位：至少3个非共线基站。
% 3D定位：至少4个非共面基站。
if sv_num < 3
return
end
while (END_LOOP > B1 && max_retry > 0) % 循环条件：位置更新量大于阈值B1且未超过最大迭代次数
% 获得当前位置与各个基站的欧氏距离
r = vecnorm(sv_pos - pos);
% 求得H矩阵
H = (sv_pos - pos) ./ r; % 求每个矩阵的单位方向向量， ./是将矩阵中每个对应元素逐个做除法运算
if dim == 2
H = [H [0 0 -1]']; % 如果是2D模式，添加虚拟高度约束
end
H =-H'; % 转置并取负，H 是伪距对位置的偏导数矩阵（雅可比矩阵），用于线性化非线性问题
% 构建残差向量dp
dp = (pr - r)'; % 伪距测量残差 = 测量值 - 预测值
if dim == 2
dp = [dp; 0]; % 如果是2D模式，添加高度残差约束
end
% 迭代用户距离（位置增量）
delta = (H'*H)^(-1)*H'*dp; % 最小二乘解算
%计算残差，以判断残差大小是否可以停止迭代
% 残差就是“测量值减去估计值”的差距，代表你当前估计的误差。
END_LOOP = norm(delta); % 欧几里得范数（向量的长度），表示这次更新的幅度。
%更新位置
pos = pos + delta;
max_retry = max_retry - 1; % 减少剩余迭代次数
%迭代失败 如果到达了最大迭代次数还没有收敛，则说明定位失败
if(max_retry == 0 && END_LOOP > 10)
pos = lpos; % 就会回滚到上一次迭代的结果
return;
end
end
end
%
% % 最小二乘法多边测距
%
% function pos = ch_multilateration(anchor_pos, pos, pr)
%
% pr = pr(1:size(anchor_pos, 2));
%
% b = vecnorm(anchor_pos).^(2) - pr.^(2);
% b = b(1:end-1) - b(end);
% b = b';
%
% A = anchor_pos - anchor_pos(:,end);
% A = A(:,1:end-1)'*2;
%
% pos = (A'*A)^(-1)*A'*b;
%
%
% end

60
lib/nav_equ_local_tan.m Normal file
View File

@ -0,0 +1,60 @@
function [p, v, q] = nav_equ_local_tan(p, v, q ,acc, gyr, dt, gN)
% 惯导解算更新，当地直角坐标系，不考虑地球自转
% p 位置 XYZ 单位 m
% v 速度 XYZ 单位 m/s
% q Qb2n姿态,四元数表示
% acc 比力， 加速度计测量值 单位 (m/s^2),
% gyr 角速度 (rad/s)]
% dt dt (s) 积分间隔如 0.01s
% gn 当地重力向量
old_v = v; % 保存当前速度，用于后续梯形积分
sf = acc; % 比力（Specific Force） = 加速度计测量值（忽略零偏和噪声）
% 姿态结算
q = att_upt(q, gyr, dt); % 通过陀螺仪数据更新四元数
% 输入当前姿态四元数和陀螺仪角速度，输出更新后的姿态四元数
% 速度解算
sf = qmulv(q, sf); % 将比力从机体系转换到导航系
sf = sf + gN; % 添加重力补偿（导航系下）
v = old_v + dt *sf; % 积分加速度得到速度
% 位置解算
p = p + (old_v + v) *dt/2; % 梯形法积分速度
end
%
%
% function x = ch_nav_equ_local_tan(x ,u, dt, gN)
%
% persistent a_old;
% if isempty(a_old)
% a_old= u(1:3);
% end
%
% old_v = x(4:6);
%
% a_new =u(1:3);
% %sf = sf + 0.5*cross(u(4:6)*dt, sf);
%
% % 姿态结算
% gyr = u(4:6);
% q_old = x(7:10);
% x(7:10) = ch_att_upt(x(7:10), gyr, dt);
% q_new = x(7:10);
%
% % 速度解算
%
% x(4:6) = old_v + ((ch_qmulv(q_new, a_new) + ch_qmulv(q_old, a_old) )/2 + gN) *dt;
%
% % 位置解算
% x(1:3) = x(1:3) + (old_v + x(4:6)) *dt/2;
% a_old = a_new;
% end
%
%

6
lib/ned2enu.m Normal file
View File

@ -0,0 +1,6 @@
function ENU = ned2enu(NED)
end

30
lib/plot/ch_plot_att.m Normal file
View File

@ -0,0 +1,30 @@
function ch_plot_att(att, varargin)
% plot 欧拉角
% att 欧拉角
% 可选参数: units: "rad" 或者 "deg", 默认 rad
defaultUnits = 'rad';
param= inputParser;
addParameter(param,'units',defaultUnits,@isstring);
param.parse(varargin{:});
r = param.Results;
if(~isempty(r.units))
defaultUnits = r.units;
end
plot(att);
title("欧拉角");
legend("X", "Y", "Z");
xlabel("解算次数");
ylabel(defaultUnits);
end

39
lib/plot/ch_plot_gps_imu_pos.m Normal file
View File

@ -0,0 +1,39 @@
% subplot£ºÊÇ·ñ¿ªÆôsubplot
function ch_plot_gps_imu_pos(varargin)
%% plot imu data
i = 1;
param= inputParser;
param.addOptional('time', []);
param.addOptional('pos', []);
param.addOptional('gnss', []);
param.parse(varargin{:});
r = param.Results;
if(r.time == 0 )
error('no time data');
end
figure;
subplot(2,1,1);
plot(r.gnss(:,2), r.gnss(:,1),'b-');
hold on;
plot(r.gnss(:,2), r.gnss(:,1),'b.');
plot(r.pos(:,2), r.pos(:,1), 'r-');
plot(r.pos(1,1), r.pos(1,2),'ks');
legend('GNSS position estimate','GNSS aided INS trajectory','Start point')
axis equal
hold off;
xlabel('X(m)'); ylabel('Y(m)'); title('Trajectory');
subplot(2,1,2);
hold on;
plot(1:length(r.gnss), -r.gnss(:,3),'b.');
plot(r.time, -r.pos(:,3),'r');
legend('GNSS estimate','GNSS aided INS estimate')
title('Height versus time'); xlabel('Time [s]'); ylabel('Height [m]');
hold off;
end

108
lib/plot/ch_plot_imu.m Normal file
View File

@ -0,0 +1,108 @@
% subplot：是否开启subplot
function ch_plot_imu(varargin)
%% plot imu data
i = 1;
param= inputParser;
param.addOptional('time', []);
param.addOptional('acc', []);
param.addOptional('gyr', []);
param.addOptional('mag', []);
param.addOptional('eul', []);
param.addOptional('gb', []); % 加速度零偏
param.addOptional('wb', []); % 陀螺零偏
param.addOptional('phi', []); %失准角
param.addOptional('P_phi', []); %失准角方差
param.addOptional('P_wb', []); %陀螺方差
param.addOptional('P_pos', []); %位置方差
param.addOptional('title', []);
param.addOptional('legend', []);
%然后将输入的参数进行处理，如果有不同于默认值的那就覆盖掉
param.parse(varargin{:});
r = param.Results;
if(r.time == 0 )
error('no time data');
end
i = 1;
figure;
if(~isempty(r.gyr))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.gyr, {'X', 'Y', 'Z'}, 'Time (s)', 'Angular rate)', 'Gyroscope');
end
if(~isempty(r.acc))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.acc, {'X', 'Y', 'Z'}, 'Time (s)', 'Acceleration', 'Accelerometer');
end
if(~isempty(r.mag))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.mag, {'X', 'Y', 'Z'}, 'Time (s)', 'Flux ', 'Magnetometer');
end
if(~isempty(r.eul))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.eul, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', 'Eular Angle');
end
if(~isempty(r.wb))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.wb, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', '陀螺零偏');
end
if(~isempty(r.gb))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.gb, {'X', 'Y', 'Z'}, 'Time (s)', 'm/s^(2)', '加速度零偏');
end
if(~isempty(r.phi))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.phi, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', '失准角');
end
if(~isempty(r.P_phi))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.P_phi, {'X', 'Y', 'Z'}, 'Time (s)', '-', 'Phi Var(失准角方差)');
end
if(~isempty(r.P_wb))
subplot(2,2,i);
i = i+1;
interial_display(r.time, r.P_wb, {'X', 'Y', 'Z'}, 'Time (s)', '-', '陀螺零偏方差');
end
if(~isempty(r.P_pos))
subplot(2,2,i);
interial_display(r.time, r.P_pos, {'X', 'Y', 'Z'}, 'Time (s)', '-', '位置方差');
end
% linkaxes(axis, 'x');
end
function interial_display(time, data, legendstr, xlabelstr, ylabelstr, titlestr)
hold on;
plot(time, data(:,1), 'r');
plot(time, data(:,2), 'g');
plot(time, data(:,3), 'b');
legend(legendstr);
xlabel(xlabelstr);
ylabel(ylabelstr);
title(titlestr);
hold off;
end

26
lib/plot/ch_plot_pos2d.m Normal file
View File

@ -0,0 +1,26 @@
function ch_plot_pos2d(pos, varargin)
% pos: 位置： X, Y
defaultUnits = 'm';
param= inputParser;
addParameter(param,'units',defaultUnits,@isstring);
param.parse(varargin{:});
r = param.Results;
if(~isempty(r.units))
defaultUnits = r.units;
end
plot(pos(:,1), pos(:,2));
title("2D位置" + "(" + defaultUnits + ")") ;
xlabel("X"); ylabel("Y");
end

25
lib/plot/ch_plot_pos3d.m Normal file
View File

@ -0,0 +1,25 @@
function ch_plot_pos3d(pos, varargin)
% pos: 位置： X, Y, Z
defaultUnits = 'm';
param= inputParser;
addParameter(param,'units',defaultUnits,@isstring);
param.parse(varargin{:});
r = param.Results;
if(~isempty(r.units))
defaultUnits = r.units;
end
plot(pos(:,1), pos(:,2));
plot3(pos(:,1), pos(:,2), pos(:,3));
title("3D位置" + "(" + defaultUnits + ")") ;
xlabel("X"); ylabel("Y"); zlabel("Z");
end

47
lib/ros_read_bag.m Normal file
View File

@ -0,0 +1,47 @@
%% Example
% path = 'imu3_codeodom_tst.bag';
%
% rosbag info 'imu3_codeodom_tst.bag'
% [imu, eul]= ch_ros_read_bag(path, '/imu_hi226');
% ch_imu_data_plot('acc', imu.acc', 'gyr', imu.gyr', 'eul', eul', 'time', imu.time', 'subplot', 1);
%%
function [imu, eul]= ros_read_bag(path, imu_topic_name)
bag = rosbag(path);
%ÏÔʾbagÐÅÏ¢
% rosbag info 'imu3_codeodom_tst.bag'
% bag = select(bag, 'Time', [bag.StartTime+100 bag.StartTime + 110], 'Topic',imu_topic_name);
bag = select(bag, 'Topic', imu_topic_name);
imu_cell = readMessages(bag);
n = length(imu_cell);
span = bag.EndTime - bag.StartTime ;
imu.time = 0: span / n : span;
imu.time(end) = []; % remove last element
for i = 1:n
imu.gyr(1,i) = imu_cell{i,1}.AngularVelocity.X;
imu.gyr(2,i) = imu_cell{i,1}.AngularVelocity.Y;
imu.gyr(3,i) = imu_cell{i,1}.AngularVelocity.Z;
imu.acc(1,i) = imu_cell{i,1}.LinearAcceleration.X;
imu.acc(2,i) = imu_cell{i,1}.LinearAcceleration.Y;
imu.acc(3,i) = imu_cell{i,1}.LinearAcceleration.Z;
q(1) = imu_cell{i, 1}.Orientation.W;
q(2) = imu_cell{i, 1}.Orientation.X;
q(3) = imu_cell{i, 1}.Orientation.Y;
q(4) = imu_cell{i, 1}.Orientation.Z;
eul(:, i) = q2eul(q);
end
eul = rad2deg(eul);
end

255
lib/rotation/SixDOFanimation.m Normal file
View File

@ -0,0 +1,255 @@
function fig = SixDOFanimation(varargin)
%% Create local variables
% Required arguments
p = varargin{1}; % position of body
R = varargin{2}; % rotation matrix of body
[numSamples dummy] = size(p);
% Default values of optional arguments
SamplePlotFreq = 1;
Trail = 'Off';
LimitRatio = 1;
Position = [];
FullScreen = false;
View = [30 20];
AxisLength = 1;
ShowArrowHead = 'on';
Xlabel = 'X';
Ylabel = 'Y';
Zlabel = 'Z';
Title = '6DOF Animation';
ShowLegend = true;
CreateAVI = false;
AVIfileName = '6DOF Animation';
AVIfileNameEnum = true;
AVIfps = 30;
for i = 3:2:nargin
if strcmp(varargin{i}, 'SamplePlotFreq'), SamplePlotFreq = varargin{i+1};
elseif strcmp(varargin{i}, 'Trail')
Trail = varargin{i+1};
if(~strcmp(Trail, 'Off') && ~strcmp(Trail, 'DotsOnly') && ~strcmp(Trail, 'All'))
error('Invalid argument. Trail must be ''Off'', ''DotsOnly'' or ''All''.');
end
elseif strcmp(varargin{i}, 'LimitRatio'), LimitRatio = varargin{i+1};
elseif strcmp(varargin{i}, 'Position'), Position = varargin{i+1};
elseif strcmp(varargin{i}, 'FullScreen'), FullScreen = varargin{i+1};
elseif strcmp(varargin{i}, 'View'), View = varargin{i+1};
elseif strcmp(varargin{i}, 'AxisLength'), AxisLength = varargin{i+1};
elseif strcmp(varargin{i}, 'ShowArrowHead'), ShowArrowHead = varargin{i+1};
elseif strcmp(varargin{i}, 'Xlabel'), Xlabel = varargin{i+1};
elseif strcmp(varargin{i}, 'Ylabel'), Ylabel = varargin{i+1};
elseif strcmp(varargin{i}, 'Zlabel'), Zlabel = varargin{i+1};
elseif strcmp(varargin{i}, 'Title'), Title = varargin{i+1};
elseif strcmp(varargin{i}, 'ShowLegend'), ShowLegend = varargin{i+1};
elseif strcmp(varargin{i}, 'CreateAVI'), CreateAVI = varargin{i+1};
elseif strcmp(varargin{i}, 'AVIfileName'), AVIfileName = varargin{i+1};
elseif strcmp(varargin{i}, 'AVIfileNameEnum'), AVIfileNameEnum = varargin{i+1};
elseif strcmp(varargin{i}, 'AVIfps'), AVIfps = varargin{i+1};
else error('Invalid argument.');
end
end;
%% Reduce data to samples to plot only
p = p(1:SamplePlotFreq:numSamples, :);
R = R(:, :, 1:SamplePlotFreq:numSamples) * AxisLength;
if(numel(View) > 2)
View = View(1:SamplePlotFreq:numSamples, :);
end
[numPlotSamples dummy] = size(p);
%% Setup AVI file
aviobj = []; % create null object
if(CreateAVI)
fileName = strcat(AVIfileName, '.avi');
if(exist(fileName, 'file'))
if(AVIfileNameEnum) % if file name exists and enum enabled
i = 0;
while(exist(fileName, 'file')) % find un-used file name by appending enum
fileName = strcat(AVIfileName, sprintf('%i', i), '.avi');
i = i + 1;
end
else % else file name exists and enum disabled
fileName = []; % file will not be created
end
end
if(isempty(fileName))
sprintf('AVI file not created as file already exists.')
else
aviobj = avifile(fileName, 'fps', AVIfps, 'compression', 'Cinepak', 'quality', 100);
end
end
%% Setup figure and plot
% Create figure
fig = figure('NumberTitle', 'off', 'Name', '6DOF Animation');
if(FullScreen)
screenSize = get(0, 'ScreenSize');
set(fig, 'Position', [0 0 screenSize(3) screenSize(4)]);
elseif(~isempty(Position))
set(fig, 'Position', Position);
end
set(gca, 'drawmode', 'fast');
lighting phong;
set(gcf, 'Renderer', 'zbuffer');
hold on;
axis equal;
grid on;
view(View(1, 1), View(1, 2));
title(i);
xlabel(Xlabel);
ylabel(Ylabel);
zlabel(Zlabel);
% Create plot data arrays
if(strcmp(Trail, 'DotsOnly') || strcmp(Trail, 'All'))
x = zeros(numPlotSamples, 1);
y = zeros(numPlotSamples, 1);
z = zeros(numPlotSamples, 1);
end
if(strcmp(Trail, 'All'))
ox = zeros(numPlotSamples, 1);
oy = zeros(numPlotSamples, 1);
oz = zeros(numPlotSamples, 1);
ux = zeros(numPlotSamples, 1);
vx = zeros(numPlotSamples, 1);
wx = zeros(numPlotSamples, 1);
uy = zeros(numPlotSamples, 1);
vy = zeros(numPlotSamples, 1);
wy = zeros(numPlotSamples, 1);
uz = zeros(numPlotSamples, 1);
vz = zeros(numPlotSamples, 1);
wz = zeros(numPlotSamples, 1);
end
x(1) = p(1,1);
y(1) = p(1,2);
z(1) = p(1,3);
ox(1) = x(1);
oy(1) = y(1);
oz(1) = z(1);
ux(1) = R(1,1,1:1);
vx(1) = R(2,1,1:1);
wx(1) = R(3,1,1:1);
uy(1) = R(1,2,1:1);
vy(1) = R(2,2,1:1);
wy(1) = R(3,2,1:1);
uz(1) = R(1,3,1:1);
vz(1) = R(2,3,1:1);
wz(1) = R(3,3,1:1);
% Create graphics handles
orgHandle = plot3(x, y, z, 'k.');
if(ShowArrowHead)
ShowArrowHeadStr = 'on';
else
ShowArrowHeadStr = 'off';
end
quivXhandle = quiver3(ox, oy, oz, ux, vx, wx, 'r', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivYhandle = quiver3(ox, oy, oz, uy, vy, wy, 'g', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
quivZhandle = quiver3(ox, ox, oz, uz, vz, wz, 'b', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');
% Create legend
if(ShowLegend)
legend('Origin', 'X', 'Y', 'Z');
end
% Set initial limits
Xlim = [x(1)-AxisLength x(1)+AxisLength] * LimitRatio;
Ylim = [y(1)-AxisLength y(1)+AxisLength] * LimitRatio;
Zlim = [z(1)-AxisLength z(1)+AxisLength] * LimitRatio;
set(gca, 'Xlim', Xlim, 'Ylim', Ylim, 'Zlim', Zlim);
% Set initial view
view(View(1, :));
%% Plot one sample at a time
for i = 1:numPlotSamples
% Update graph title
if(strcmp(Title, ''))
titleText = sprintf('Sample %i of %i', 1+((i-1)*SamplePlotFreq), numSamples);
else
titleText = strcat(Title, ' (', sprintf('Sample %i of %i', 1+((i-1)*SamplePlotFreq), numSamples), ')');
end
title(titleText);
% Plot body x y z axes
if(strcmp(Trail, 'DotsOnly') || strcmp(Trail, 'All'))
x(1:i) = p(1:i,1);
y(1:i) = p(1:i,2);
z(1:i) = p(1:i,3);
else
x = p(i,1);
y = p(i,2);
z = p(i,3);
end
if(strcmp(Trail, 'All'))
ox(1:i) = p(1:i,1);
oy(1:i) = p(1:i,2);
oz(1:i) = p(1:i,3);
ux(1:i) = R(1,1,1:i);
vx(1:i) = R(2,1,1:i);
wx(1:i) = R(3,1,1:i);
uy(1:i) = R(1,2,1:i);
vy(1:i) = R(2,2,1:i);
wy(1:i) = R(3,2,1:i);
uz(1:i) = R(1,3,1:i);
vz(1:i) = R(2,3,1:i);
wz(1:i) = R(3,3,1:i);
else
ox = p(i,1);
oy = p(i,2);
oz = p(i,3);
ux = R(1,1,i);
vx = R(2,1,i);
wx = R(3,1,i);
uy = R(1,2,i);
vy = R(2,2,i);
wy = R(3,2,i);
uz = R(1,3,i);
vz = R(2,3,i);
wz = R(3,3,i);
end
set(orgHandle, 'xdata', x, 'ydata', y, 'zdata', z);
set(quivXhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', ux, 'vdata', vx, 'wdata', wx);
set(quivYhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', uy, 'vdata', vy, 'wdata', wy);
set(quivZhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', uz, 'vdata', vz, 'wdata', wz);
% Adjust axes for snug fit and draw
axisLimChanged = false;
if((p(i,1) - AxisLength) < Xlim(1)), Xlim(1) = p(i,1) - LimitRatio*AxisLength; axisLimChanged = true; end
if((p(i,2) - AxisLength) < Ylim(1)), Ylim(1) = p(i,2) - LimitRatio*AxisLength; axisLimChanged = true; end
if((p(i,3) - AxisLength) < Zlim(1)), Zlim(1) = p(i,3) - LimitRatio*AxisLength; axisLimChanged = true; end
if((p(i,1) + AxisLength) > Xlim(2)), Xlim(2) = p(i,1) + LimitRatio*AxisLength; axisLimChanged = true; end
if((p(i,2) + AxisLength) > Ylim(2)), Ylim(2) = p(i,2) + LimitRatio*AxisLength; axisLimChanged = true; end
if((p(i,3) + AxisLength) > Zlim(2)), Zlim(2) = p(i,3) + LimitRatio*AxisLength; axisLimChanged = true; end
if(axisLimChanged), set(gca, 'Xlim', Xlim, 'Ylim', Ylim, 'Zlim', Zlim); end
drawnow;
% Adjust view
if(numel(View) > 2)
view(View(i, :));
end
% Add frame to AVI object
if(~isempty(aviobj))
frame = getframe(fig);
aviobj = addframe(aviobj, frame);
end
end
hold off;
% Close AVI file
if(~isempty(aviobj))
aviobj = close(aviobj);
end
end

23
lib/rotation/eul2m.m Normal file
View File

@ -0,0 +1,23 @@
function [Cb2n_312, Cb2n_321] = eul2m(att)
% 将欧拉角转换为姿态阵
% 复用严龚敏老师的a2mat
%
% Input: att 单位：rad
% 对于312((Z->X->Y))顺序，对应att = [pitch(绕X轴) roll(绕Y轴) yaw(绕Z轴)]
% 对于3211(Z->Y->X)顺序，对应att = [roll(绕X轴) pitch(绕Y轴) yaw(绕Z轴)]
% Outputs:
% Cb2n_312: 312欧拉角顺序下转换后的Cb2n
% Cb2n_321: 321欧拉角顺序下转换后的Cb2n
s = sin(att); c = cos(att);
si = s(1); sj = s(2); sk = s(3);
ci = c(1); cj = c(2); ck = c(3);
Cb2n_312 = [ cj*ck-si*sj*sk, -ci*sk, sj*ck+si*cj*sk;
cj*sk+si*sj*ck, ci*ck, sj*sk-si*cj*ck;
-ci*sj, si, ci*cj ];
if nargout==2 % dual Euler angle DCM
Cb2n_321 = [ cj*ck, si*sj*ck-ci*sk, ci*sj*ck+si*sk;
cj*sk, si*sj*sk+ci*ck, ci*sj*sk-si*ck;
-sj, si*cj, ci*cj ];
end

21
lib/rotation/eul2q.m Normal file
View File

@ -0,0 +1,21 @@
function [Qb2n_312, Qb2n_321] = eul2q(eul)
[Cb2n_312, Cb2n_321] = eul2m(eul);
Qb2n_312 = m2q(Cb2n_312);
Qb2n_321 = m2q(Cb2n_321);
% r = eul(1);
% p = eul(2);
% y = eul(3);
% cp = cos(p / 2);
% sp = sin(p / 2);
% cy = cos(y / 2);
% sy = sin(y / 2);
% cr = cos(r / 2);
% sr = sin(r / 2);
%
% q(1) = cr*cp*cy + sr*sp*sy;
% q(2) = -cr*sp*sy + cp*cy*sr;
% q(3) = cr*cy*sp + sr*cp*sy;
% q(4) = cr*cp*sy - sr*cy*sp;
% q = q';
% Ends

16
lib/rotation/m2eul.m Normal file
View File

@ -0,0 +1,16 @@
function [eul_312, eul_321] = m2eul(Cb2n)
% 将姿态阵转为欧拉角
% 复用严龚敏老师的m2att
%
% Input: 姿态阵Cb2n: b系->n系的坐标变换矩阵
% Outputs:
% eul_312: 312(Z->X->Y)旋转顺序下的欧拉角: att = [pitch(绕X轴) roll(绕Y轴) yaw(绕Z轴)]
% eul_321: 321(Z->Y->X)旋转顺序下的欧拉角: att = [roll(绕X轴) pitch(绕Y轴) yaw(绕Z轴)]
eul_312 = [ asin(Cb2n(3,2));
atan2(-Cb2n(3,1),Cb2n(3,3));
atan2(-Cb2n(1,2),Cb2n(2,2)) ];
if nargout==2 % dual Euler angles
eul_321 = [ atan2(Cb2n(3,2),Cb2n(3,3));
asin(-Cb2n(3,1));
atan2(Cb2n(2,1),Cb2n(1,1)) ];
end

24
lib/rotation/m2q.m Normal file
View File

@ -0,0 +1,24 @@
function Qb2n = m2q(Cb2n)
% 姿态阵转四元数
%
% Input: Cb2n
% Output: Qb2n
%
C11 = Cb2n(1,1); C12 = Cb2n(1,2); C13 = Cb2n(1,3);
C21 = Cb2n(2,1); C22 = Cb2n(2,2); C23 = Cb2n(2,3);
C31 = Cb2n(3,1); C32 = Cb2n(3,2); C33 = Cb2n(3,3);
if C11>=C22+C33
q1 = 0.5*sqrt(1+C11-C22-C33);
q0 = (C32-C23)/(4*q1); q2 = (C12+C21)/(4*q1); q3 = (C13+C31)/(4*q1);
elseif C22>=C11+C33
q2 = 0.5*sqrt(1-C11+C22-C33);
q0 = (C13-C31)/(4*q2); q1 = (C12+C21)/(4*q2); q3 = (C23+C32)/(4*q2);
elseif C33>=C11+C22
q3 = 0.5*sqrt(1-C11-C22+C33);
q0 = (C21-C12)/(4*q3); q1 = (C13+C31)/(4*q3); q2 = (C23+C32)/(4*q3);
else
q0 = 0.5*sqrt(1+C11+C22+C33);
q1 = (C32-C23)/(4*q0); q2 = (C13-C31)/(4*q0); q3 = (C21-C12)/(4*q0);
end
Qb2n = [q0; q1; q2; q3];

8
lib/rotation/mnormlz.m Normal file
View File

@ -0,0 +1,8 @@
function Cb2n = mnormlz(Cb2n)
% 姿态阵单位化，防止矩阵蠕变
for k=1:5
% Cnb = 0.5 * (Cnb + (Cnb')^-1); % Algorithm 1
Cb2n = 1.5*Cb2n - 0.5*(Cb2n*Cb2n')*Cb2n; % Algorithm 2
end
% Algorithm 3: [s,v,d] = svd(Cnb); Cnb = s*d'; % in = s*v*d' = s*d' * d*v*d'; out = s*d'

27
lib/rotation/q2eul.m Normal file
View File

@ -0,0 +1,27 @@
function [eul_312, eul_321] = q2eul(Qb2n)
% 四元数转欧拉角
%
% Input: 四元数Qb2n
% Outputs:
% eul_312: 312(Z->X->Y)旋转顺序下的欧拉角: att = [pitch(绕X轴) roll(绕Y轴) yaw(绕Z轴)]
% eul_321: 321(Z->Y->X)旋转顺序下的欧拉角: att = [roll(绕X轴) pitch(绕Y轴) yaw(绕Z轴)]
q0 = Qb2n(1);
q1 = Qb2n(2);
q2 = Qb2n(3);
q3 = Qb2n(4);
roll = -atan2( 2*( q1*q3 - q0*q2 ) , q0*q0 - q1*q1 - q2*q2 + q3*q3);
pitch = asin( 2*(q0*q1 + q2*q3) );
yaw = -atan2(2*( q1*q2 - q0*q3 ), q0*q0 - q1*q1 + q2*q2 - q3*q3);
eul_312 = [pitch; roll; yaw];
if nargout==2 % dual Euler angles
roll = atan2( 2*( q0*q1 + q2*q3 ) , 1 - 2*q1*q1 - 2*q2*q2);
pitch = asin( 2*(q0*q2 - q1*q3) );
yaw = atan2(2*( q0*q3 + q1*q2 ), 1 - 2*q2*q2 - 2*q3*q3);
eul_321 = [roll; pitch; yaw];
end

15
lib/rotation/q2m.m Normal file
View File

@ -0,0 +1,15 @@
function Cb2n = q2m(Qb2n)
% 四元数转姿态阵
%
% Input: Qb2n
% Output: Cb2n
%
q11 = Qb2n(1)*Qb2n(1); q12 = Qb2n(1)*Qb2n(2); q13 = Qb2n(1)*Qb2n(3); q14 = Qb2n(1)*Qb2n(4);
q22 = Qb2n(2)*Qb2n(2); q23 = Qb2n(2)*Qb2n(3); q24 = Qb2n(2)*Qb2n(4);
q33 = Qb2n(3)*Qb2n(3); q34 = Qb2n(3)*Qb2n(4);
q44 = Qb2n(4)*Qb2n(4);
Cb2n = [ q11+q22-q33-q44, 2*(q23-q14), 2*(q24+q13);
2*(q23+q14), q11-q22+q33-q44, 2*(q34-q12);
2*(q24-q13), 2*(q34+q12), q11-q22-q33+q44 ];

2
lib/rotation/qconj.m Normal file
View File

@ -0,0 +1,2 @@
function qout = qconj(qin)
qout = [qin(1); -qin(2:4)];

10
lib/rotation/qmul.m Normal file
View File

@ -0,0 +1,10 @@
function q = qmul(q1, q2)
% 四元数相乘
%
% Inputs: Q1 Q2, 四元数和矩阵一样，不满足交换律
% Outputs: Q
%
q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4);
q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ];

24
lib/rotation/qmulv.m Normal file
View File

@ -0,0 +1,24 @@
function vo = qmulv(q, vi)
% 向量通过四元数做3D旋转
%
% Inputs: q - Qb2n
% vi - 需要旋转的向量
% Output: vout - output vector, such that vout = q*vin*conjugation(q)
%
% See also q2mat, qconj, qmul.
% qi = [0; vi];
% qo = ch_qmul(ch_qmul(q,qi),ch_qconj(q));
% vo = qo(2:4,1);
qo1 = - q(2) * vi(1) - q(3) * vi(2) - q(4) * vi(3);
qo2 = q(1) * vi(1) + q(3) * vi(3) - q(4) * vi(2);
qo3 = q(1) * vi(2) + q(4) * vi(1) - q(2) * vi(3);
qo4 = q(1) * vi(3) + q(2) * vi(2) - q(3) * vi(1);
vo = vi;
vo(1) = -qo1 * q(2) + qo2 * q(1) - qo3 * q(4) + qo4 * q(3);
vo(2) = -qo1 * q(3) + qo3 * q(1) - qo4 * q(2) + qo2 * q(4);
vo(3) = -qo1 * q(4) + qo4 * q(1) - qo2 * q(3) + qo3 * q(2);

9
lib/rotation/qnormlz.m Normal file
View File

@ -0,0 +1,9 @@
function q = qnormlz(q)
% 四元数归一化
q = q/norm(q);
if(q(1)<0)
q(1) = -q(1);
q(2) = -q(2);
q(3) = -q(3);
q(4) = -q(4);
end

5
lib/rotation/rotx.m Normal file
View File

@ -0,0 +1,5 @@
function Cb2n = rotx(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
end

5
lib/rotation/roty.m Normal file
View File

@ -0,0 +1,5 @@
function Cb2n = roty(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [cos(theta), 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];
end

5
lib/rotation/rotz.m Normal file
View File

@ -0,0 +1,5 @@
function Cb2n = rotz(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];
end

19
lib/rotation/rv2m.m Normal file
View File

@ -0,0 +1,19 @@
function m = rv2m(rv)
% 等效旋转矢量转换为旋转矩阵
%
% Input: rv - 旋转矢量
% Output: m -旋转矩阵 Cb2n 严龚敏 2.2.23
% m = I + sin(|rv|)/|rv|*(rvx) + [1-cos(|rv|)]/|rv|^2*(rvx)^2
% where rvx is the askew matrix or rv.
nm2 = rv'*rv; % 旋转矢量的模方
if nm2<1.e-8 % 如果模方很小，则可用泰勒展开前几项求三角函数
a = 1-nm2*(1/6-nm2/120); b = 0.5-nm2*(1/24-nm2/720); % a->1, b->0.5
else
nm = sqrt(nm2);
a = sin(nm)/nm; b = (1-cos(nm))/nm2;
end
VX = askew(rv);
m = eye(3) + a*VX + b*VX^2;

12
lib/rotation/rv2q.m Normal file
View File

@ -0,0 +1,12 @@
function q = rv2q(rv) % 等效旋转矢量转换为变换四元数
nm2 = rv'*rv; % 旋转矢量的模方
if nm2<1.0e-8 % 如果模方很小，则可用泰勒展开前几项求三角函数
q0 = 1-nm2*(1/8-nm2/384);
s = 1/2-nm2*(1/48-nm2/3840);
else
nm = sqrt(nm2);
q0 = cos(nm/2);
s = sin(nm/2)/nm;
end
q = [q0; s*rv];

18
lib/rotation/uv2q.m Normal file
View File

@ -0,0 +1,18 @@
%%
%Cartographer SLAM 姿态算法分析
%刘兴华
%电邮:xiphix@126.com
%微信:xiphix
function [q] = uv2q(v1, v2)
%Finding quaternion representing the rotation from one vector to another
nv1 = v1/norm(v1);
nv2 = v2/norm(v2);
if norm(nv1+nv2)==0
q = [1 0 0 0]';
else
half = (nv1 + nv2)/norm(nv1 + nv2);
q = [nv1'*half; cross(nv1, half)];
end
end

17
lib/sv2atti.m Normal file
View File

@ -0,0 +1,17 @@
function [Cb2n] = sv2atti(acc)
acc = acc / norm(acc);
Cb2n(3,:) = acc ;
C3 = Cb2n(3,:);
if Cb2n(3,1) > 0.5
C2 = [Cb2n(3,2), -Cb2n(3,1), 0];
else
C2 = [0, Cb2n(3,3), -Cb2n(3,2)];
end
C2 = C2 / norm(C2);
C1 = cross(C2, C3);
Cb2n = [C1; C2; C3];
end

21
lib/triangulate.m Normal file
View File

@ -0,0 +1,21 @@
% 迭代式多边测距
function p = triangulate(anchor_pos, p, pr)
% 基站个数
n = size(anchor_pos, 2);
% 获得当前位置与各个基站的距离
r = vecnorm(anchor_pos - p);
% 求得H矩阵
H = (anchor_pos - p) ./ r;
H =-H';
% 迭代用户距离
p = p + (H'*H)^(-1)*H'*(pr - r)';
end