commit fd57c4cc628a6c1f734a15abded6172aeb39ff21 Author: ZhaiShuaiShuai <18264798931@163.com> Date: Wed Apr 16 20:15:33 2025 +0800 first commit diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..c530b64 --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +study/eskf156/dataset/**/*.mat filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bf25acf --- /dev/null +++ b/.gitignore @@ -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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..f7498e6 --- /dev/null +++ b/README.md @@ -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. 然后再入组合导航知识 diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas1.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas1.mat new file mode 100644 index 0000000..a2660de Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas1.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas2.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas2.mat new file mode 100644 index 0000000..cab4bf4 Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas2.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas3.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas3.mat new file mode 100644 index 0000000..26d323c Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas3.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas4.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas4.mat new file mode 100644 index 0000000..708f806 Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas4.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas5.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas5.mat new file mode 100644 index 0000000..fbcf653 Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas5.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas6.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas6.mat new file mode 100644 index 0000000..81104b7 Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas6.mat differ diff --git a/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas7.mat b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas7.mat new file mode 100644 index 0000000..26548eb Binary files /dev/null and b/UWBInsFusion/Datasets/NTV/TWR-4A1T/datas7.mat differ diff --git a/UWBInsFusion/README.md b/UWBInsFusion/README.md new file mode 100644 index 0000000..c438b99 --- /dev/null +++ b/UWBInsFusion/README.md @@ -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。 diff --git a/UWBInsFusion/demo_plot.m b/UWBInsFusion/demo_plot.m new file mode 100644 index 0000000..66f6ac5 --- /dev/null +++ b/UWBInsFusion/demo_plot.m @@ -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 + + + + + diff --git a/UWBInsFusion/eskf_uwb_imu.m b/UWBInsFusion/eskf_uwb_imu.m new file mode 100644 index 0000000..444faf3 --- /dev/null +++ b/UWBInsFusion/eskf_uwb_imu.m @@ -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 diff --git a/UWBInsFusion/uwb_imu_example_settings.m b/UWBInsFusion/uwb_imu_example_settings.m new file mode 100644 index 0000000..847c27c --- /dev/null +++ b/UWBInsFusion/uwb_imu_example_settings.m @@ -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 + + + + diff --git a/lib/ENU2LLA.m b/lib/ENU2LLA.m new file mode 100644 index 0000000..00e1865 --- /dev/null +++ b/lib/ENU2LLA.m @@ -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 \ No newline at end of file diff --git a/lib/allan.m b/lib/allan.m new file mode 100644 index 0000000..490399c --- /dev/null +++ b/lib/allan.m @@ -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 \ No newline at end of file diff --git a/lib/askew.m b/lib/askew.m new file mode 100644 index 0000000..7b38960 --- /dev/null +++ b/lib/askew.m @@ -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 ]; + \ No newline at end of file diff --git a/lib/att_upt.m b/lib/att_upt.m new file mode 100644 index 0000000..8b64179 --- /dev/null +++ b/lib/att_upt.m @@ -0,0 +1,44 @@ + +function out = att_upt(in, gyr, dt) +% 뵱ǰ̬Ԫǽٶȣº̬Ԫ +% inΪǰ̬Ԫ +% gyrDzֵ +% 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 + diff --git a/lib/calbiration/acc_calibration.m b/lib/calbiration/acc_calibration.m new file mode 100644 index 0000000..06fe93b --- /dev/null +++ b/lib/calbiration/acc_calibration.m @@ -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 diff --git a/lib/calbiration/ch_magcal2d.m b/lib/calbiration/ch_magcal2d.m new file mode 100644 index 0000000..0c32ef8 --- /dev/null +++ b/lib/calbiration/ch_magcal2d.m @@ -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 + + + +% diff --git a/lib/calbiration/ellipsoid_fit.m b/lib/calbiration/ellipsoid_fit.m new file mode 100644 index 0000000..fbe8e27 --- /dev/null +++ b/lib/calbiration/ellipsoid_fit.m @@ -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 + diff --git a/lib/calbiration/ellipsoid_fit_new.m b/lib/calbiration/ellipsoid_fit_new.m new file mode 100644 index 0000000..f857e87 --- /dev/null +++ b/lib/calbiration/ellipsoid_fit_new.m @@ -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 + + + + diff --git a/lib/calbiration/gyr_calibration.m b/lib/calbiration/gyr_calibration.m new file mode 100644 index 0000000..2ad1d13 --- /dev/null +++ b/lib/calbiration/gyr_calibration.m @@ -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 + diff --git a/lib/data_import.m b/lib/data_import.m new file mode 100644 index 0000000..eb819d5 --- /dev/null +++ b/lib/data_import.m @@ -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 + diff --git a/lib/dip13.m b/lib/dip13.m new file mode 100644 index 0000000..9f6bc11 --- /dev/null +++ b/lib/dip13.m @@ -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 + + \ No newline at end of file diff --git a/lib/dip5.m b/lib/dip5.m new file mode 100644 index 0000000..e54eeb4 --- /dev/null +++ b/lib/dip5.m @@ -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 + + \ No newline at end of file diff --git a/lib/dpi.m b/lib/dpi.m new file mode 100644 index 0000000..72945af --- /dev/null +++ b/lib/dpi.m @@ -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]) + + + diff --git a/lib/dv2atti.m b/lib/dv2atti.m new file mode 100644 index 0000000..61a43a4 --- /dev/null +++ b/lib/dv2atti.m @@ -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)]'; + + \ No newline at end of file diff --git a/lib/geo/ch_ECEF2ENU.m b/lib/geo/ch_ECEF2ENU.m new file mode 100644 index 0000000..bec43ad --- /dev/null +++ b/lib/geo/ch_ECEF2ENU.m @@ -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 diff --git a/lib/geo/ch_ECEF2LLA.m b/lib/geo/ch_ECEF2LLA.m new file mode 100644 index 0000000..b4a1af7 --- /dev/null +++ b/lib/geo/ch_ECEF2LLA.m @@ -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 diff --git a/lib/geo/ch_ENU2ECEF.m b/lib/geo/ch_ENU2ECEF.m new file mode 100644 index 0000000..4f6fdc7 --- /dev/null +++ b/lib/geo/ch_ENU2ECEF.m @@ -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 diff --git a/lib/geo/ch_ENU2LLA.m b/lib/geo/ch_ENU2LLA.m new file mode 100644 index 0000000..bdae44b --- /dev/null +++ b/lib/geo/ch_ENU2LLA.m @@ -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 \ No newline at end of file diff --git a/lib/geo/ch_LLA2ECEF.m b/lib/geo/ch_LLA2ECEF.m new file mode 100644 index 0000000..21781e7 --- /dev/null +++ b/lib/geo/ch_LLA2ECEF.m @@ -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; diff --git a/lib/geo/ch_LLA2ENU.m b/lib/geo/ch_LLA2ENU.m new file mode 100644 index 0000000..943ef9c --- /dev/null +++ b/lib/geo/ch_LLA2ENU.m @@ -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 diff --git a/lib/geo/ch_earth.m b/lib/geo/ch_earth.m new file mode 100644 index 0000000..40f912b --- /dev/null +++ b/lib/geo/ch_earth.m @@ -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: ECEFENUת +% C_ECEF2NED: ECEFNEDת + + +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 \ No newline at end of file diff --git a/lib/geo/ch_gravity.m b/lib/geo/ch_gravity.m new file mode 100644 index 0000000..0b45be9 --- /dev/null +++ b/lib/geo/ch_gravity.m @@ -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)); + + diff --git a/lib/gnss/UTC2GPST.m b/lib/gnss/UTC2GPST.m new file mode 100644 index 0000000..047ba75 --- /dev/null +++ b/lib/gnss/UTC2GPST.m @@ -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; + diff --git a/lib/gnss/anheader.m b/lib/gnss/anheader.m new file mode 100644 index 0000000..79dfad4 --- /dev/null +++ b/lib/gnss/anheader.m @@ -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 %%%%%%%%% diff --git a/lib/gnss/cal2gpstime.m b/lib/gnss/cal2gpstime.m new file mode 100644 index 0000000..9526296 --- /dev/null +++ b/lib/gnss/cal2gpstime.m @@ -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; + diff --git a/lib/gnss/ch_gpsdop.m b/lib/gnss/ch_gpsdop.m new file mode 100644 index 0000000..4bfd99c --- /dev/null +++ b/lib/gnss/ch_gpsdop.m @@ -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 diff --git a/lib/gnss/ch_gpsls.m b/lib/gnss/ch_gpsls.m new file mode 100644 index 0000000..47e847d --- /dev/null +++ b/lib/gnss/ch_gpsls.m @@ -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 + + diff --git a/lib/gnss/ch_sat_pos.m b/lib/gnss/ch_sat_pos.m new file mode 100644 index 0000000..0a8f814 --- /dev/null +++ b/lib/gnss/ch_sat_pos.m @@ -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 \ No newline at end of file diff --git a/lib/gnss/ch_sv_pos_rotate.m b/lib/gnss/ch_sv_pos_rotate.m new file mode 100644 index 0000000..f4f61a3 --- /dev/null +++ b/lib/gnss/ch_sv_pos_rotate.m @@ -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 diff --git a/lib/gnss/check_rinex_line_length.m b/lib/gnss/check_rinex_line_length.m new file mode 100644 index 0000000..64b1a13 --- /dev/null +++ b/lib/gnss/check_rinex_line_length.m @@ -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'); \ No newline at end of file diff --git a/lib/gnss/fepoch_0.m b/lib/gnss/fepoch_0.m new file mode 100644 index 0000000..50f142a --- /dev/null +++ b/lib/gnss/fepoch_0.m @@ -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ַΪ00Ԫ + % ֻܳ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˵ + % һijһԪĿʼУͿԴӸȡʱ䡢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 + + % prn1NoSvеľ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 %%%%%%%%% diff --git a/lib/gnss/get_eph.m b/lib/gnss/get_eph.m new file mode 100644 index 0000000..1f65abf --- /dev/null +++ b/lib/gnss/get_eph.m @@ -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Ϊһ23noephеľ +%%%%%%%% end get_eph.m %%%%%%%%%%%%%%%%%%%%% diff --git a/lib/gnss/iono_correction.m b/lib/gnss/iono_correction.m new file mode 100644 index 0000000..5664789 --- /dev/null +++ b/lib/gnss/iono_correction.m @@ -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Ϣй㲥IJGPS 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 frequencyL1ƵʵбУ(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; diff --git a/lib/gnss/parsef.m b/lib/gnss/parsef.m new file mode 100644 index 0000000..aec8ce8 --- /dev/null +++ b/lib/gnss/parsef.m @@ -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 + 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 + diff --git a/lib/gnss/read_rinex_nav.m b/lib/gnss/read_rinex_nav.m new file mode 100644 index 0000000..f46d592 --- /dev/null +++ b/lib/gnss/read_rinex_nav.m @@ -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 + diff --git a/lib/gnss/read_rinex_obs.m b/lib/gnss/read_rinex_obs.m new file mode 100644 index 0000000..a1a5a4c --- /dev/null +++ b/lib/gnss/read_rinex_obs.m @@ -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 + diff --git a/lib/gnss/rinexe.m b/lib/gnss/rinexe.m new file mode 100644 index 0000000..4aa3af2 --- /dev/null +++ b/lib/gnss/rinexe.m @@ -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 +% Ϊÿ1noephе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 %%%%%%%%% + \ No newline at end of file diff --git a/lib/gnss/satellite_az_el.m b/lib/gnss/satellite_az_el.m new file mode 100644 index 0000000..b714a4c --- /dev/null +++ b/lib/gnss/satellite_az_el.m @@ -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 + + diff --git a/lib/gnss/set_constants.m b/lib/gnss/set_constants.m new file mode 100644 index 0000000..2abb17e --- /dev/null +++ b/lib/gnss/set_constants.m @@ -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 +% ========================================================================= + diff --git a/lib/gnss/sv_clock_bias.m b/lib/gnss/sv_clock_bias.m new file mode 100644 index 0000000..88d6ff2 --- /dev/null +++ b/lib/gnss/sv_clock_bias.m @@ -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 \ No newline at end of file diff --git a/lib/gnss/tropo_correction.m b/lib/gnss/tropo_correction.m new file mode 100644 index 0000000..7a27676 --- /dev/null +++ b/lib/gnss/tropo_correction.m @@ -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 \ No newline at end of file diff --git a/lib/madgwick.m b/lib/madgwick.m new file mode 100644 index 0000000..3a924b4 --- /dev/null +++ b/lib/madgwick.m @@ -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 \ No newline at end of file diff --git a/lib/mahony.m b/lib/mahony.m new file mode 100644 index 0000000..9ab8d64 --- /dev/null +++ b/lib/mahony.m @@ -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 \ No newline at end of file diff --git a/lib/mat2c.m b/lib/mat2c.m new file mode 100644 index 0000000..72497f9 --- /dev/null +++ b/lib/mat2c.m @@ -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 diff --git a/lib/multilateration.m b/lib/multilateration.m new file mode 100644 index 0000000..70a9ba7 --- /dev/null +++ b/lib/multilateration.m @@ -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; % С˽ + + %вжϲвСǷֹͣ + % вǡֵȥֵIJ࣬㵱ǰƵ + END_LOOP = norm(delta); % ŷ÷ijȣʾθµķȡ + + %λ + 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 + diff --git a/lib/nav_equ_local_tan.m b/lib/nav_equ_local_tan.m new file mode 100644 index 0000000..4989401 --- /dev/null +++ b/lib/nav_equ_local_tan.m @@ -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 +% +% diff --git a/lib/ned2enu.m b/lib/ned2enu.m new file mode 100644 index 0000000..a3bc3d7 --- /dev/null +++ b/lib/ned2enu.m @@ -0,0 +1,6 @@ +function ENU = ned2enu(NED) + + + +end + diff --git a/lib/plot/ch_plot_att.m b/lib/plot/ch_plot_att.m new file mode 100644 index 0000000..53058a8 --- /dev/null +++ b/lib/plot/ch_plot_att.m @@ -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 + diff --git a/lib/plot/ch_plot_gps_imu_pos.m b/lib/plot/ch_plot_gps_imu_pos.m new file mode 100644 index 0000000..9abc6dd --- /dev/null +++ b/lib/plot/ch_plot_gps_imu_pos.m @@ -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 + diff --git a/lib/plot/ch_plot_imu.m b/lib/plot/ch_plot_imu.m new file mode 100644 index 0000000..b50fe4b --- /dev/null +++ b/lib/plot/ch_plot_imu.m @@ -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', []); + + +%ȻIJдвͬĬֵǾ͸ǵ +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 diff --git a/lib/plot/ch_plot_pos2d.m b/lib/plot/ch_plot_pos2d.m new file mode 100644 index 0000000..c5e8d0c --- /dev/null +++ b/lib/plot/ch_plot_pos2d.m @@ -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 + + diff --git a/lib/plot/ch_plot_pos3d.m b/lib/plot/ch_plot_pos3d.m new file mode 100644 index 0000000..2bf8528 --- /dev/null +++ b/lib/plot/ch_plot_pos3d.m @@ -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 + + diff --git a/lib/ros_read_bag.m b/lib/ros_read_bag.m new file mode 100644 index 0000000..deed07f --- /dev/null +++ b/lib/ros_read_bag.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/SixDOFanimation.m b/lib/rotation/SixDOFanimation.m new file mode 100644 index 0000000..2999fa0 --- /dev/null +++ b/lib/rotation/SixDOFanimation.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/eul2m.m b/lib/rotation/eul2m.m new file mode 100644 index 0000000..9df8afc --- /dev/null +++ b/lib/rotation/eul2m.m @@ -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 + \ No newline at end of file diff --git a/lib/rotation/eul2q.m b/lib/rotation/eul2q.m new file mode 100644 index 0000000..600bd32 --- /dev/null +++ b/lib/rotation/eul2q.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/m2eul.m b/lib/rotation/m2eul.m new file mode 100644 index 0000000..c2487d2 --- /dev/null +++ b/lib/rotation/m2eul.m @@ -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 diff --git a/lib/rotation/m2q.m b/lib/rotation/m2q.m new file mode 100644 index 0000000..2ced892 --- /dev/null +++ b/lib/rotation/m2q.m @@ -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]; + diff --git a/lib/rotation/mnormlz.m b/lib/rotation/mnormlz.m new file mode 100644 index 0000000..2e561a5 --- /dev/null +++ b/lib/rotation/mnormlz.m @@ -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' \ No newline at end of file diff --git a/lib/rotation/q2eul.m b/lib/rotation/q2eul.m new file mode 100644 index 0000000..5e1c257 --- /dev/null +++ b/lib/rotation/q2eul.m @@ -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 + + diff --git a/lib/rotation/q2m.m b/lib/rotation/q2m.m new file mode 100644 index 0000000..1c49186 --- /dev/null +++ b/lib/rotation/q2m.m @@ -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 ]; + + \ No newline at end of file diff --git a/lib/rotation/qconj.m b/lib/rotation/qconj.m new file mode 100644 index 0000000..3363b14 --- /dev/null +++ b/lib/rotation/qconj.m @@ -0,0 +1,2 @@ +function qout = qconj(qin) +qout = [qin(1); -qin(2:4)]; \ No newline at end of file diff --git a/lib/rotation/qmul.m b/lib/rotation/qmul.m new file mode 100644 index 0000000..e102670 --- /dev/null +++ b/lib/rotation/qmul.m @@ -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) ]; diff --git a/lib/rotation/qmulv.m b/lib/rotation/qmulv.m new file mode 100644 index 0000000..54bc58d --- /dev/null +++ b/lib/rotation/qmulv.m @@ -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); + \ No newline at end of file diff --git a/lib/rotation/qnormlz.m b/lib/rotation/qnormlz.m new file mode 100644 index 0000000..1f5aefc --- /dev/null +++ b/lib/rotation/qnormlz.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/rotx.m b/lib/rotation/rotx.m new file mode 100644 index 0000000..cd58eeb --- /dev/null +++ b/lib/rotation/rotx.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/roty.m b/lib/rotation/roty.m new file mode 100644 index 0000000..c8de625 --- /dev/null +++ b/lib/rotation/roty.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/rotz.m b/lib/rotation/rotz.m new file mode 100644 index 0000000..986feaa --- /dev/null +++ b/lib/rotation/rotz.m @@ -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 \ No newline at end of file diff --git a/lib/rotation/rv2m.m b/lib/rotation/rv2m.m new file mode 100644 index 0000000..480e5a7 --- /dev/null +++ b/lib/rotation/rv2m.m @@ -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; + + \ No newline at end of file diff --git a/lib/rotation/rv2q.m b/lib/rotation/rv2q.m new file mode 100644 index 0000000..b57da30 --- /dev/null +++ b/lib/rotation/rv2q.m @@ -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]; + diff --git a/lib/rotation/uv2q.m b/lib/rotation/uv2q.m new file mode 100644 index 0000000..9ad7b35 --- /dev/null +++ b/lib/rotation/uv2q.m @@ -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 \ No newline at end of file diff --git a/lib/sv2atti.m b/lib/sv2atti.m new file mode 100644 index 0000000..f9373cf --- /dev/null +++ b/lib/sv2atti.m @@ -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 \ No newline at end of file diff --git a/lib/triangulate.m b/lib/triangulate.m new file mode 100644 index 0000000..c83f2ca --- /dev/null +++ b/lib/triangulate.m @@ -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 + +