first commit
commit
fd57c4cc62
|
|
@ -0,0 +1 @@
|
|||
study/eskf156/dataset/**/*.mat filter=lfs diff=lfs merge=lfs -text
|
||||
|
|
@ -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
|
||||
|
|
@ -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. 然后再入组合导航知识
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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。
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 ];
|
||||
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
|
||||
function out = att_upt(in, gyr, dt)
|
||||
% 输入当前姿态四元数和陀螺仪角速度，输出更新后的姿态四元数
|
||||
% in为当前姿态四元数
|
||||
% gyr是陀螺仪测量值
|
||||
% dt 时间步长
|
||||
|
||||
%% 单子样旋转矢量
|
||||
rv = gyr*dt; % 旋转矢量 = 角速度 × 时间步长
|
||||
dq = rv2q(rv); % 调用函数将旋转矢量转为四元数
|
||||
|
||||
%% 不专业的做法
|
||||
% dq(1) = 1;
|
||||
% dq(2) = rv(1)*0.5;
|
||||
% dq(3) = rv(2)*0.5;
|
||||
% dq(4) = rv(3)*0.5;
|
||||
|
||||
out = qmul(in, dq); % 四元数相乘 当前姿态 × 增量 = 新姿态
|
||||
out = qnormlz(out); % 归一化 归一化防止数值漂移
|
||||
|
||||
%% 使用旋转矩阵更新
|
||||
%
|
||||
% Cb2n = ch_q2m(in);
|
||||
% theta = gyr*dt;
|
||||
%
|
||||
% %C = eye(3) + ch_askew(theta);
|
||||
% C = ch_rv2m(theta);
|
||||
%
|
||||
% Cb2n = Cb2n * C;
|
||||
%
|
||||
% % 截断误差，保持正交化 GNSS与惯性及多传感器组合导航系统原理-第二版.pdf 公式 5.80
|
||||
% c1 = Cb2n(1,:);
|
||||
% c2 = Cb2n(2,:);
|
||||
% c3 = Cb2n(3,:);
|
||||
% c1 = 2 / (1 + dot(c1,c1))*c1;
|
||||
% c2 = 2 / (1 + dot(c2,c2))*c2;
|
||||
% c3 = 2 / (1 + dot(c3,c3))*c3;
|
||||
% Cb2n = [c1; c2; c3];
|
||||
%
|
||||
% out = ch_m2q(Cb2n);
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
%
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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])
|
||||
|
||||
|
||||
|
||||
|
|
@ -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)]';
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
|
||||
function [Rns, Rew, C_ECEF2ENU, C_ECEF2NED]= ch_earth(lat, lon, h)
|
||||
|
||||
%% 根据经纬度计算地球常用参数
|
||||
% INPUT
|
||||
% lat: 纬度(rad)
|
||||
% lon: 经度(rad)
|
||||
|
||||
% OUTPUT
|
||||
% Rns, R_meridian(RM, ns)： 南北向地球曲率半径, 子午圈曲率半径(竖着的)
|
||||
% Rew_transverse(RN, ew)：东西向地球曲率半径, 卯酉圈曲率半径(横着的)
|
||||
% C_ECEF2ENU: ECEF到ENU转换矩阵
|
||||
% C_ECEF2NED: ECEF到NED转换矩阵
|
||||
|
||||
|
||||
R0 = 6378137; %WGS84 赤道半径
|
||||
e = 0.0818191908425; %WGS84 eccentricity
|
||||
% Calculate meridian radius of curvature using (2.105)
|
||||
temp = 1 - (e * sin(lat))^2;
|
||||
Rns = R0 * (1 - e^2) / temp^1.5;
|
||||
|
||||
% Calculate transverse radius of curvature using (2.105)
|
||||
Rew = R0 / sqrt(temp);
|
||||
|
||||
clat = cos(lat);
|
||||
slat = sin(lat);
|
||||
clon = cos(lon);
|
||||
slon = sin(lon);
|
||||
|
||||
C_ECEF2ENU(1,:) = [-slon , clon, 0];
|
||||
C_ECEF2ENU(2,:) = [ -slat*clon, -slat*slon clat];
|
||||
C_ECEF2ENU(3,:) = [ clat*clon, clat*slon, slat];
|
||||
|
||||
|
||||
C_ECEF2NED(1,:) = [-slat*clon, -slat * slon, clat];
|
||||
C_ECEF2NED(2,:) = [-slon, clon, 0];
|
||||
C_ECEF2NED(3,:) = [ -clat*clon, -clat*slon, -slat];
|
||||
|
||||
|
||||
end
|
||||
|
|
@ -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));
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
@ -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 %%%%%%%%%
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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');
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
function [time, dt, sats, eof] = fepoch_0(fid)
|
||||
% FEPOCH_0 Finds the next epoch in an opened RINEX file with
|
||||
% identification fid. From the epoch line is produced
|
||||
% time (in seconds of week), number of sv.s, and a mark
|
||||
% about end of file. Only observations with epoch flag 0
|
||||
% are delt with.
|
||||
|
||||
%Kai Borre 09-14-96; revised 03-22-97; revised Sept 4, 2001
|
||||
%Copyright (c) by Kai Borre
|
||||
%$Revision: 1.0 $ $Date: 1997/09/22 $
|
||||
%fide = fopen(fid,'rt');
|
||||
|
||||
% 该函数只返回ｏ文件中一个历元的以下参数：
|
||||
|
||||
% time：周内秒
|
||||
% dt：接收机钟差（该参数为可选项，不一定所有的o文件中都有）
|
||||
% sats：当前历元所观测到的卫星
|
||||
% eof：是否到文件末尾
|
||||
|
||||
global sat_index;
|
||||
time = 0;
|
||||
dt = 0;
|
||||
sats = [];
|
||||
NoSv = 0;
|
||||
eof = 0;
|
||||
|
||||
%循环读取o文件的每一行
|
||||
while 1
|
||||
lin = fgets(fid); % earlier fgetl 先读一行
|
||||
|
||||
if (feof(fid) == 1); % 若到文件末尾，则结束
|
||||
eof = 1;
|
||||
break
|
||||
end;
|
||||
|
||||
% 跳过空行
|
||||
if length(lin) <= 1
|
||||
continue;
|
||||
end
|
||||
|
||||
% answer = findstr(lin,'COMMENT'); % 判断该行中是否有字符串“COMMENT”
|
||||
%
|
||||
% if ~isempty(answer); % 若有“COMMENT“，则继续读下一行
|
||||
% lin = fgetl(fid);
|
||||
% end;
|
||||
|
||||
% 如果该行数据第29个字符不为0（0代表改历元正常），
|
||||
% 或者总长度只有29（也就是没有后面的卫星PRN数据），
|
||||
% 则结束。
|
||||
% if ((strcmp(lin(29),'0') == 0) & (size(deblank(lin),2) == 29))
|
||||
% eof = 1;
|
||||
% break
|
||||
% end; % We only want type 0 data
|
||||
|
||||
% 如果该行第二个字符是1，或者第29个字符是0，则说明
|
||||
% 这一行是某一历元的开始行，接下来就可以从该行中提取时间、PRN等参数
|
||||
if ((strcmp(lin(2),'1') == 1) & (strcmp(lin(29),'0') == 1))
|
||||
ll = length(lin)-2;
|
||||
if ll > 60, ll = 60; end;
|
||||
linp = lin(1:ll);
|
||||
%fprintf('%60s\n',linp);
|
||||
|
||||
%使用strtok函数获得间隔符前面的字符串，
|
||||
% 首先是获得当前时间
|
||||
[nian, lin] = strtok(lin);
|
||||
% year;
|
||||
|
||||
[month, lin] = strtok(lin);
|
||||
% month;
|
||||
|
||||
[day, lin] = strtok(lin);
|
||||
% day;
|
||||
|
||||
[hour, lin] = strtok(lin);
|
||||
% hour
|
||||
|
||||
[minute, lin] = strtok(lin);
|
||||
% minute
|
||||
|
||||
[second, lin] = strtok(lin);
|
||||
% second
|
||||
|
||||
[OK_flag, lin] = strtok(lin);
|
||||
% OK_flag就是第29个字符，如果是0，则该历元正常
|
||||
|
||||
%将时间转成数值型，然后再计算出GPS周和周内秒
|
||||
h = str2num(hour)+str2num(minute)/60+str2num(second)/3600;
|
||||
jd = julday(str2num(nian)+2000, str2num(month), str2num(day), h);
|
||||
[week, sec_of_week] = gps_time(jd);
|
||||
time = sec_of_week;
|
||||
|
||||
%获得该历元卫星数
|
||||
[NoSv, lin] = strtok(lin,'G');
|
||||
|
||||
%储存该历元每颗卫星的PRN
|
||||
for k = 1:str2num(NoSv)
|
||||
[sat, lin] = strtok(lin,'G');
|
||||
prn(k) = str2num(sat);
|
||||
end
|
||||
|
||||
% prn是1行NoSv列的矩阵，sats是其转置
|
||||
sats = prn(:);
|
||||
|
||||
% 接收及钟差，有的o文件中没有该参数
|
||||
dT = strtok(lin);
|
||||
if isempty(dT) == 0 %如果dT不为0，则将其记录
|
||||
dt = str2num(dT);
|
||||
end
|
||||
|
||||
break % 跳出while循环
|
||||
|
||||
end
|
||||
|
||||
end;
|
||||
|
||||
% datee=[str2num(nian) str2num(month) str2num(day) str2num(hour) str2num(minute) str2num(second)];
|
||||
|
||||
%%%%%%%% end fepoch_0.m %%%%%%%%%
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
function eph = get_eph(ephemeridesfile)
|
||||
%GET_EPH The ephemerides contained in ephemeridesfile
|
||||
% are reshaped into a matrix with 21 rows and
|
||||
% as many columns as there are ephemerides.
|
||||
|
||||
% Typical call eph = get_eph('rinex_n.dat')
|
||||
|
||||
%Kai Borre 10-10-96
|
||||
%Copyright (c) by Kai Borre
|
||||
%$Revision: 1.0 $ $Date: 1997/09/26 $
|
||||
|
||||
fide = fopen(ephemeridesfile);
|
||||
[eph, count] = fread(fide, Inf, 'double');
|
||||
noeph = count/23;
|
||||
eph = reshape(eph, 23, noeph); % ephΪһ¸ö23ÐÐnoephÁеľØÕó
|
||||
%%%%%%%% end get_eph.m %%%%%%%%%%%%%%%%%%%%%
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
%This Function approximate Ionospheric Group Delay 这个函数近似电离层群延迟
|
||||
%CopyRight By Moein Mehrtash
|
||||
%************************************************************************
|
||||
% Written by Moein Mehrtash, Concordia University, 3/21/2008 *
|
||||
% Email: moeinmehrtash@yahoo.com *
|
||||
%************************************************************************
|
||||
% ***********************************************************************
|
||||
% Function for computing an Ionospheric range correction for the *
|
||||
% GPS L1 frequency from the parameters broadcasted in the GPS *
|
||||
% Navigation Message.功能:根据GPS导航信息中广播的参数计算电离层距离修正的GPS L1频率。 *
|
||||
% ==================================================================
|
||||
% References: *
|
||||
% Klobuchar, J.A., (1996) "Ionosphercic Effects on GPS", in *
|
||||
% Parkinson, Spilker (ed), "Global Positioning System Theory and *
|
||||
% Applications, pp.513-514. *
|
||||
% ICD-GPS-200, Rev. C, (1997), pp. 125-128 *
|
||||
% NATO, (1991), "Technical Characteristics of the NAVSTAR GPS", *
|
||||
% pp. A-6-31 - A-6-33 *
|
||||
% ==================================================================
|
||||
% Input : *
|
||||
% Pos_Rcv : XYZ position of reciever (Meter) *
|
||||
% Pos_SV : XYZ matrix position of GPS satellites (Meter) *
|
||||
% GPS_Time : Time of Week (sec) *
|
||||
% Alfa(4) : The coefficients of a cubic equation *
|
||||
% representing the amplitude of the vertical *
|
||||
% dalay (4 coefficients - 8 bits each) 表示垂直dalay振幅的三次方程的系数(4个系数，每个8位) *
|
||||
% Beta(4) : The coefficients of a cubic equation *
|
||||
% representing the period of the model *
|
||||
% (4 coefficients - 8 bits each) 表示模型周期的三次方程的系数(4个系数-各8位) *
|
||||
% Output: *
|
||||
% Delta_I : Ionospheric slant range correction for *
|
||||
% the L1 frequency电离层L1频率的倾斜距离校正(Sec) *
|
||||
% ==================================================================
|
||||
|
||||
function [ delay ]=iono_correction(RP, SP, alpha, beta, gpst)
|
||||
% RP: 接收机位置
|
||||
% SP: 卫星位置
|
||||
% Alpha Beta 电离层校准参数
|
||||
% GPS Time
|
||||
% delay 单位为m
|
||||
|
||||
if norm(RP) < 1
|
||||
delay = 0;
|
||||
return;
|
||||
end
|
||||
|
||||
[lat, lon, ~] = ch_ECEF2LLA(RP);
|
||||
|
||||
|
||||
lat = lat/pi;
|
||||
lon =lon/pi; % semicircles unit Lattitdue and Longitude
|
||||
|
||||
|
||||
|
||||
[el, az] = satellite_az_el(SP, RP);
|
||||
E = az/pi; %SemiCircle Elevation
|
||||
A = el; %SemiCircle Azimoth
|
||||
% Calculate the Earth-Centered angle, Psi
|
||||
Psi = 0.0137/(E+.11)-0.022; %SemiCircle
|
||||
|
||||
%Compute the Subionospheric lattitude, Phi_L
|
||||
Phi_L = lat+Psi*cos(A); %SemiCircle
|
||||
if Phi_L>0.416
|
||||
Phi_L=0.416;
|
||||
elseif Phi_L<-0.416
|
||||
Phi_L=-0.416;
|
||||
end
|
||||
|
||||
%Compute the subionospheric longitude, Lambda_L
|
||||
Lambda_L = lon+(Psi * sin(A)/cos(Phi_L*pi)); %SemiCircle
|
||||
|
||||
%Find the geomagnetic lattitude ,Phi_m, of the subionospheric location
|
||||
%looking toward each GPS satellite:
|
||||
Phi_m = Phi_L+0.064*cos((Lambda_L-1.617)*pi);
|
||||
|
||||
%Find the Local Time ,t, at the subionospheric point
|
||||
t=4.32*10^4*Lambda_L+gpst; %GPS_Time(Sec)
|
||||
t= mod(t,86400);
|
||||
if t>86400
|
||||
t = t-86400;
|
||||
elseif t<0
|
||||
t=t+86400;
|
||||
end
|
||||
|
||||
%Convert Slant time delay, Compute the Slant Factor,F
|
||||
F=1+16*(.53-E)^3;
|
||||
|
||||
%Compute the ionospheric time delay T_iono by first computing x
|
||||
Per=beta(1)+beta(2)*Phi_m+beta(3)*Phi_m^2+beta(4)*Phi_m^3;
|
||||
if Per <72000 %Period
|
||||
Per=72000;
|
||||
end
|
||||
x=2*pi*(t-50400)/Per; %Rad
|
||||
AMP=alpha(1)+alpha(2)*Phi_m+alpha(3)*Phi_m^2+alpha(4)*Phi_m^3;
|
||||
if AMP<0
|
||||
AMP=0;
|
||||
end
|
||||
if abs(x)>1.57
|
||||
T_iono=F*5*10^(-9);
|
||||
else
|
||||
T_iono=F*(5*10^(-9)+AMP*(1-x^2/2+x^4/24));
|
||||
end
|
||||
|
||||
|
||||
delay = T_iono*299792458;
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
function varargout = parsef(input, format)
|
||||
%parsef parse string value using FORTRAN formatting codes
|
||||
% [val1,val2,...valn] = parsef(input, format)
|
||||
% input is string input value
|
||||
% format is cell array of format codes
|
||||
|
||||
global input_
|
||||
input_ = input;
|
||||
varargout = getvals(1, format, 1);
|
||||
clear global input_
|
||||
return
|
||||
|
||||
% this function does the work. you probably don't want to go here
|
||||
function [output, idx] = getvals(idx, format, reps)
|
||||
global input_
|
||||
count = 1;
|
||||
output = {};
|
||||
for k = 1:reps
|
||||
odx = 1;
|
||||
for i = 1:length(format)
|
||||
fmt = format{i};
|
||||
switch class(fmt)
|
||||
case 'double'
|
||||
count = fmt;
|
||||
case 'char'
|
||||
type = fmt(1);
|
||||
if type == 'X'
|
||||
idx = idx+count;
|
||||
else
|
||||
[len,cnt] = sscanf(fmt,'%*c%d',1);
|
||||
if cnt ~= 1
|
||||
error(['Invalid format specifier: ''',fmt,'''']);
|
||||
end
|
||||
switch type
|
||||
case {'I','i'}
|
||||
for j = 1:count
|
||||
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%d',1);
|
||||
if cnt == 1
|
||||
output{odx}(j,k) = val;
|
||||
else
|
||||
output{odx}(j,k) = NaN;
|
||||
end
|
||||
idx = idx+len;
|
||||
end
|
||||
case {'F','f'}
|
||||
for j = 1:count
|
||||
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%f',1);
|
||||
if cnt == 1
|
||||
output{odx}(j,k) = val;
|
||||
else
|
||||
output{odx}(j,k) = NaN;
|
||||
end
|
||||
idx = idx+len;
|
||||
end
|
||||
case {'E','D','G'}
|
||||
for j = 1:count
|
||||
[val,cnt] = sscanf(input_(idx:min(idx+len-1,end)),'%f%*1[DdEe]%f',2);
|
||||
if cnt == 2
|
||||
output{odx}(j,k) = val(1) * 10^val(2); %#ok<AGROW>
|
||||
elseif cnt == 1
|
||||
output{odx}(j,k) = val;
|
||||
else
|
||||
output{odx}(j,k) = NaN;
|
||||
end
|
||||
idx = idx+len;
|
||||
end
|
||||
case 'A'
|
||||
for j = 1:count
|
||||
output{odx}{j,k} = input_(idx:min(idx+len-1,end));
|
||||
idx = idx+len;
|
||||
end
|
||||
otherwise
|
||||
error(['Invalid format specifier: ''',fmt,'''']);
|
||||
end
|
||||
odx = odx+1;
|
||||
end
|
||||
count = 1;
|
||||
case 'cell'
|
||||
[val, idx] = getvals(idx, fmt, count);
|
||||
if length(val) == 1
|
||||
output(odx) = val;
|
||||
else
|
||||
output{odx} = val;
|
||||
end
|
||||
odx = odx+1;
|
||||
count = 1;
|
||||
end
|
||||
end
|
||||
end
|
||||
return
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -0,0 +1,204 @@
|
|||
function iono = rinexe(ephemerisfile, outputfile)
|
||||
%RINEXE Reads a RINEX Navigation Message file and
|
||||
% reformats the data into a matrix with 23s
|
||||
% rows and a column for each satellite.
|
||||
% The matrix is stored in outputfile
|
||||
|
||||
%Typical call: rinexe('pta.96n','pta.nav')
|
||||
|
||||
%Kai Borre 04-18-96
|
||||
%Copyright (c) by Kai Borre
|
||||
%$Revision: 1.0 $ $Date: 1997/09/24 $
|
||||
|
||||
% Units are either seconds, meters, or radians
|
||||
fide = fopen(ephemerisfile);
|
||||
head_lines = 0;
|
||||
iono = zeros(8,1);
|
||||
|
||||
%read the header
|
||||
header_end = [];
|
||||
while (isempty(header_end))
|
||||
head_lines = head_lines+1;
|
||||
%read the line and search the ionosphere labels
|
||||
lin = fgetl(fide);
|
||||
|
||||
vers_found = ~isempty(strfind(lin,'RINEX VERSION / TYPE'));
|
||||
iono_found = (~isempty(strfind(lin,'ION ALPHA')) || ~isempty(strfind(lin,'IONOSPHERIC CORR')));
|
||||
|
||||
%if the ionosphere parameters label was found
|
||||
if (vers_found)
|
||||
version = str2num(lin(1:9));
|
||||
end
|
||||
|
||||
%if the ionosphere parameters label was found
|
||||
if (iono_found)
|
||||
%change flag
|
||||
% ioparam = 1;
|
||||
%save the 8 ionosphere parameters
|
||||
data = textscan(lin(5:end),'%f%f%f%f%*[^\n]');
|
||||
if ~isempty(data(4))
|
||||
iono(1) = data{1};
|
||||
iono(2) = data{2};
|
||||
iono(3) = data{3};
|
||||
iono(4) = data{4};
|
||||
lin = [];
|
||||
while isempty(lin)
|
||||
lin = fgetl(fide);
|
||||
end
|
||||
data = textscan(lin(5:end),'%f%f%f%f%*[^\n]');
|
||||
if ~isempty(data(4))
|
||||
iono(5) = data{1};
|
||||
iono(6) = data{2};
|
||||
iono(7) = data{3};
|
||||
iono(8) = data{4};
|
||||
else
|
||||
iono = zeros(8,1);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
header_end = strfind(lin,'END OF HEADER');
|
||||
end
|
||||
head_lines = head_lines + 1;
|
||||
noeph = -1; %初始化星历数目
|
||||
while 1
|
||||
noeph = noeph+1;
|
||||
line = fgetl(fide);
|
||||
if line == -1, break; end
|
||||
end;
|
||||
noeph = noeph/8 % 卫星星历数 = 去掉文件头之后的文件行数 / 8
|
||||
% 每颗卫星星历行数是8行
|
||||
|
||||
% 下面两行作用是回到文件头结束的地方
|
||||
frewind(fide);
|
||||
for i = 1:head_lines, line = fgetl(fide); end;
|
||||
|
||||
% Set aside memory for the input，
|
||||
% 为星历中每个参数产生1行noeph列的0矩阵，
|
||||
% 每一列对应该星历文件中的一个PRN
|
||||
noeph = fix(noeph);
|
||||
svprn = zeros(1,noeph);
|
||||
weekno = zeros(1,noeph);
|
||||
t0c = zeros(1,noeph);
|
||||
tgd = zeros(1,noeph);
|
||||
aodc = zeros(1,noeph);
|
||||
toe = zeros(1,noeph);
|
||||
af2 = zeros(1,noeph);
|
||||
af1 = zeros(1,noeph);
|
||||
af0 = zeros(1,noeph);
|
||||
aode = zeros(1,noeph);
|
||||
deltan = zeros(1,noeph);
|
||||
M0 = zeros(1,noeph);
|
||||
ecc = zeros(1,noeph);
|
||||
roota = zeros(1,noeph);
|
||||
toe = zeros(1,noeph);
|
||||
cic = zeros(1,noeph);
|
||||
crc = zeros(1,noeph);
|
||||
cis = zeros(1,noeph);
|
||||
crs = zeros(1,noeph);
|
||||
cuc = zeros(1,noeph);
|
||||
cus = zeros(1,noeph);
|
||||
Omega0 = zeros(1,noeph);
|
||||
omega = zeros(1,noeph);
|
||||
i0 = zeros(1,noeph);
|
||||
Omegadot = zeros(1,noeph);
|
||||
idot = zeros(1,noeph);
|
||||
accuracy = zeros(1,noeph);
|
||||
health = zeros(1,noeph);
|
||||
fit_interval = zeros(1,noeph);
|
||||
|
||||
% 将所有卫星的星历参数存入上面定义的矩阵中
|
||||
for i = 1:noeph
|
||||
line = fgetl(fide); % 从该组星历中第一颗卫星的星历的第一行开始
|
||||
svprn(i) = str2num(line(1:2)); % 存入卫星PRN
|
||||
year = line(3:6);
|
||||
month = line(7:9);
|
||||
day = line(10:12);
|
||||
hour = line(13:15);
|
||||
minute = line(16:18);
|
||||
second = line(19:22);
|
||||
af0(i) = str2num(line(23:41));
|
||||
af1(i) = str2num(line(42:60));
|
||||
af2(i) = str2num(line(61:79));
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
IODE = line(4:22);
|
||||
crs(i) = str2num(line(23:41));
|
||||
deltan(i) = str2num(line(42:60));
|
||||
M0(i) = str2num(line(61:79));
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
cuc(i) = str2num(line(4:22));
|
||||
ecc(i) = str2num(line(23:41));
|
||||
cus(i) = str2num(line(42:60));
|
||||
roota(i) = str2num(line(61:79));
|
||||
|
||||
line=fgetl(fide); % 读下一行
|
||||
toe(i) = str2num(line(4:22));
|
||||
cic(i) = str2num(line(23:41));
|
||||
Omega0(i) = str2num(line(42:60));
|
||||
cis(i) = str2num(line(61:79));
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
i0(i) = str2num(line(4:22));
|
||||
crc(i) = str2num(line(23:41));
|
||||
omega(i) = str2num(line(42:60));
|
||||
Omegadot(i) = str2num(line(61:79));
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
idot(i) = str2num(line(4:22));
|
||||
codes = str2num(line(23:41));
|
||||
weekno = str2num(line(42:60));
|
||||
L2flag = str2num(line(61:79));
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
svaccur = str2num(line(4:22));
|
||||
svhealth(i) = str2num(line(23:41));
|
||||
tgd(i) = str2num(line(42:60));
|
||||
iodc = line(61:79);
|
||||
|
||||
line = fgetl(fide); % 读下一行
|
||||
if length(line)>= 41
|
||||
tom(i) = str2num(line(4:22));
|
||||
fit_interval(i) = str2num(line(23:41));
|
||||
else
|
||||
tom(i) = str2num(line(4:22));
|
||||
fit_interval(i) = 0;
|
||||
end
|
||||
% spare = line(42:60);
|
||||
% spare = line(61:79);
|
||||
end
|
||||
status = fclose(fide)
|
||||
|
||||
% Description of variable eph.
|
||||
% 把上面存好的各参数再转存到一个大矩阵“eph”中，该矩阵每行代表一种参数
|
||||
% 每列代表一颗卫星
|
||||
eph(1,:) = svprn;
|
||||
eph(2,:) = af2;
|
||||
eph(3,:) = M0;
|
||||
eph(4,:) = roota;
|
||||
eph(5,:) = deltan;
|
||||
eph(6,:) = ecc;
|
||||
eph(7,:) = omega;
|
||||
eph(8,:) = cuc;
|
||||
eph(9,:) = cus;
|
||||
eph(10,:) = crc;
|
||||
eph(11,:) = crs;
|
||||
eph(12,:) = i0;
|
||||
eph(13,:) = idot;
|
||||
eph(14,:) = cic;
|
||||
eph(15,:) = cis;
|
||||
eph(16,:) = Omega0;
|
||||
eph(17,:) = Omegadot;
|
||||
eph(18,:) = toe;
|
||||
eph(19,:) = af0;
|
||||
eph(20,:) = af1;
|
||||
eph(21,:) = toe;
|
||||
eph(22,:) = fit_interval;
|
||||
eph(23,:) = svhealth;
|
||||
|
||||
fidu = fopen(outputfile,'w');
|
||||
count = fwrite(fidu,[eph],'double');
|
||||
fclose all
|
||||
%%%%%%%%% end rinexe.m %%%%%%%%%
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
% =========================================================================
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
function pos = multilateration(sv_pos, pos, pr, dim)
|
||||
%% 最小二乘法多边测距 解算标签位置
|
||||
% M x N: M:维度 2 OR 3 N:基站个数
|
||||
% sv_pos: 基站位置 M x N
|
||||
% pos: M x 1
|
||||
% pr: 伪距 N x 1
|
||||
% dim : 2 or 3 : 2: 二维定位 3: 三维定位
|
||||
|
||||
B1 = 0.1; % 迭代收敛阈值（位置更新量小于此值时停止）
|
||||
END_LOOP = 100; % 初始迭代残差（设为较大值）
|
||||
sv_num = size(sv_pos, 2); % 基站数量
|
||||
max_retry = 5; % 最大迭代次数
|
||||
lpos = pos; % 保存上一次的位置，用于迭代失败时回退
|
||||
|
||||
% 设置约束，如果基站数量少于3，则直接退出。
|
||||
% 因为解算位置需满足以下条件：
|
||||
% 2D定位：至少3个非共线基站。
|
||||
% 3D定位：至少4个非共面基站。
|
||||
if sv_num < 3
|
||||
return
|
||||
end
|
||||
|
||||
|
||||
while (END_LOOP > B1 && max_retry > 0) % 循环条件：位置更新量大于阈值B1且未超过最大迭代次数
|
||||
% 获得当前位置与各个基站的欧氏距离
|
||||
r = vecnorm(sv_pos - pos);
|
||||
|
||||
% 求得H矩阵
|
||||
H = (sv_pos - pos) ./ r; % 求每个矩阵的单位方向向量， ./是将矩阵中每个对应元素逐个做除法运算
|
||||
if dim == 2
|
||||
H = [H [0 0 -1]']; % 如果是2D模式，添加虚拟高度约束
|
||||
end
|
||||
H =-H'; % 转置并取负，H 是伪距对位置的偏导数矩阵（雅可比矩阵），用于线性化非线性问题
|
||||
|
||||
% 构建残差向量dp
|
||||
dp = (pr - r)'; % 伪距测量残差 = 测量值 - 预测值
|
||||
if dim == 2
|
||||
dp = [dp; 0]; % 如果是2D模式，添加高度残差约束
|
||||
end
|
||||
|
||||
% 迭代用户距离（位置增量）
|
||||
delta = (H'*H)^(-1)*H'*dp; % 最小二乘解算
|
||||
|
||||
%计算残差，以判断残差大小是否可以停止迭代
|
||||
% 残差就是“测量值减去估计值”的差距，代表你当前估计的误差。
|
||||
END_LOOP = norm(delta); % 欧几里得范数（向量的长度），表示这次更新的幅度。
|
||||
|
||||
%更新位置
|
||||
pos = pos + delta;
|
||||
max_retry = max_retry - 1; % 减少剩余迭代次数
|
||||
|
||||
%迭代失败 如果到达了最大迭代次数还没有收敛，则说明定位失败
|
||||
if(max_retry == 0 && END_LOOP > 10)
|
||||
pos = lpos; % 就会回滚到上一次迭代的结果
|
||||
return;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
||||
%
|
||||
% % 最小二乘法多边测距
|
||||
%
|
||||
% function pos = ch_multilateration(anchor_pos, pos, pr)
|
||||
%
|
||||
% pr = pr(1:size(anchor_pos, 2));
|
||||
%
|
||||
% b = vecnorm(anchor_pos).^(2) - pr.^(2);
|
||||
% b = b(1:end-1) - b(end);
|
||||
% b = b';
|
||||
%
|
||||
% A = anchor_pos - anchor_pos(:,end);
|
||||
% A = A(:,1:end-1)'*2;
|
||||
%
|
||||
% pos = (A'*A)^(-1)*A'*b;
|
||||
%
|
||||
%
|
||||
% end
|
||||
|
||||
|
|
@ -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
|
||||
%
|
||||
%
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
function ENU = ned2enu(NED)
|
||||
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
% subplot：是否开启subplot
|
||||
function ch_plot_imu(varargin)
|
||||
%% plot imu data
|
||||
i = 1;
|
||||
param= inputParser;
|
||||
param.addOptional('time', []);
|
||||
param.addOptional('acc', []);
|
||||
param.addOptional('gyr', []);
|
||||
param.addOptional('mag', []);
|
||||
param.addOptional('eul', []);
|
||||
param.addOptional('gb', []); % 加速度零偏
|
||||
param.addOptional('wb', []); % 陀螺零偏
|
||||
param.addOptional('phi', []); %失准角
|
||||
param.addOptional('P_phi', []); %失准角方差
|
||||
param.addOptional('P_wb', []); %陀螺方差
|
||||
param.addOptional('P_pos', []); %位置方差
|
||||
param.addOptional('title', []);
|
||||
param.addOptional('legend', []);
|
||||
|
||||
|
||||
%然后将输入的参数进行处理，如果有不同于默认值的那就覆盖掉
|
||||
param.parse(varargin{:});
|
||||
r = param.Results;
|
||||
|
||||
if(r.time == 0 )
|
||||
error('no time data');
|
||||
end
|
||||
i = 1;
|
||||
|
||||
figure;
|
||||
|
||||
if(~isempty(r.gyr))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.gyr, {'X', 'Y', 'Z'}, 'Time (s)', 'Angular rate)', 'Gyroscope');
|
||||
end
|
||||
|
||||
if(~isempty(r.acc))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.acc, {'X', 'Y', 'Z'}, 'Time (s)', 'Acceleration', 'Accelerometer');
|
||||
end
|
||||
|
||||
if(~isempty(r.mag))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.mag, {'X', 'Y', 'Z'}, 'Time (s)', 'Flux ', 'Magnetometer');
|
||||
end
|
||||
|
||||
if(~isempty(r.eul))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.eul, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', 'Eular Angle');
|
||||
end
|
||||
|
||||
if(~isempty(r.wb))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.wb, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', '陀螺零偏');
|
||||
end
|
||||
|
||||
if(~isempty(r.gb))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.gb, {'X', 'Y', 'Z'}, 'Time (s)', 'm/s^(2)', '加速度零偏');
|
||||
end
|
||||
|
||||
if(~isempty(r.phi))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.phi, {'X', 'Y', 'Z'}, 'Time (s)', 'Angle(deg)', '失准角');
|
||||
end
|
||||
|
||||
if(~isempty(r.P_phi))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.P_phi, {'X', 'Y', 'Z'}, 'Time (s)', '-', 'Phi Var(失准角方差)');
|
||||
end
|
||||
|
||||
|
||||
if(~isempty(r.P_wb))
|
||||
subplot(2,2,i);
|
||||
i = i+1;
|
||||
interial_display(r.time, r.P_wb, {'X', 'Y', 'Z'}, 'Time (s)', '-', '陀螺零偏方差');
|
||||
end
|
||||
|
||||
|
||||
if(~isempty(r.P_pos))
|
||||
subplot(2,2,i);
|
||||
interial_display(r.time, r.P_pos, {'X', 'Y', 'Z'}, 'Time (s)', '-', '位置方差');
|
||||
end
|
||||
|
||||
% linkaxes(axis, 'x');
|
||||
|
||||
end
|
||||
|
||||
|
||||
function interial_display(time, data, legendstr, xlabelstr, ylabelstr, titlestr)
|
||||
hold on;
|
||||
plot(time, data(:,1), 'r');
|
||||
plot(time, data(:,2), 'g');
|
||||
plot(time, data(:,3), 'b');
|
||||
legend(legendstr);
|
||||
xlabel(xlabelstr);
|
||||
ylabel(ylabelstr);
|
||||
title(titlestr);
|
||||
hold off;
|
||||
end
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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];
|
||||
|
||||
|
|
@ -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'
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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 ];
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
function qout = qconj(qin)
|
||||
qout = [qin(1); -qin(2:4)];
|
||||
|
|
@ -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) ];
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
||||
|
||||
|
|
@ -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];
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue