%捷联惯导静态仿真主程序
% 捷联惯导（Inertial Navigation System, INS）是一种利用加速度计和陀螺仪等惯性传感器测量飞行器位置、速度和姿态的导航系统。下面是捷联惯导仿真的一般步骤：
% 1. 定义仿真参数：确定仿真的时间段、时间步长和初始条件，包括飞行器的初始位置、速度和姿态。
% 2. 生成惯性传感器数据：使用模型或随机生成的数据，生成惯性传感器（加速度计和陀螺仪）的测量数据。这些数据包括加速度计的三轴加速度和陀螺仪的三轴角速度。
% 3. 运动模型更新：根据惯性传感器的测量数据，使用运动模型（通常是动力学方程）更新飞行器的位置、速度和姿态。
% 4. 误差模型引入：根据实际情况，引入传感器误差模型。这包括加速度计和陀螺仪的偏置、尺度因子误差、随机噪声等。
% 5. 误差补偿和滤波：使用滤波算法（如卡尔曼滤波）对测量数据进行误差补偿和优化，以提高导航精度。这涉及估计和校正传感器误差，融合惯性测量和其他导航传感器（如GPS）的数据。
% 6. 导航输出：根据更新后的姿态、位置和速度，生成导航输出，例如飞行器的轨迹、姿态角度、速度矢量等。
% 7. 分析和评估：对仿真结果进行分析和评估，包括与真实数据的比较、误差评估和性能分析等。
%东北天坐标系，东X北Y天Z
clear
clc
glvs; % 加载全局变量
% 子样数和采样时间
nn = 2; % 子样数，下面也将采用二子样的圆锥误差补偿算法
ts = 0.1; % 单个采样间隔的采样时间长度
nts = nn * ts; % 采样周期，即在nts时间内进行两次采样，每次采样0.1秒
%姿态、速度和位置初始化
att = [0; 0; 30] *arcdeg; % 初始化姿态角，分别为俯仰角θ、横滚角γ、航向角ψ
vn = [0; 0; 0]; % 初始化速度
pos = [34*arcdeg; 108*arcdeg; 100]; % 初始化位置，纬度34度，经度108度，高度100m
qnb = a2qua(att); % 姿态角转换为四元数 b to n 将姿态角转换为四元数可以更方便地进行姿态的计算、插值和滤波等操作，提高了姿态运算的准确性和效率。
%仿真静态IMU数据
eth = earth(pos, vn); % 地球导航参数计算
wm = qmulv(qconj(qnb), eth.wnie) *ts; % 地球自转角速度由导航系换算到机体系，作为机体角速度，并乘时间得角度增量 四元数的旋转操作。
vm = qmulv(qconj(qnb), -eth.gn) *ts; % 比力由导航系换算到机体系，作为机体比力，并乘时间得速度增量
wm = repmat(wm', nn, 1); % 转置角速度矢量，并复制得到nn行'1'列 在每个采样时间段内重复使用相同的角速度增量。
vm = repmat(vm', nn, 1); % 转置速度矢量，并复制得到nn行'1'列
%仿真时长
phi = [0.1; 0.2; 3] *arcmin; % 失准角误差
qnb = qaddphi(qnb, phi); % 添加失准角误差的由机体系到导航系的四元数 考虑失准角误差的姿态四元数。
len = fix(3600/ts); % 仿真时长一小时，得到fix(3600/ts)个子样
%记录导航结果 [att, vn, pos, t]
avp = zeros(len, 10); %该矩阵用于存储导航结果，每一行表示一个时间段内的姿态、速度和位置信息。
kk = 1; %跟踪当前存储数据的行数
t = 0; %记录当前的时间
% 共有len个子样，每个采样时间段nn个子样，共len/nn个时间段，循环len/nn步
for k = 1:nn:len
t = t +nts; % t表示采样时间段 更新当前的时间
[qnb, vn, pos] = insupdate(qnb, vn, pos, wm, vm, ts);
vn(3) = 0; % 高度不变
avp(kk,:) = [q2att(qnb); vn; pos; t]'; % 四元数转换为姿态角
kk = kk +1;
if mod(t, 500) < nts %每过500个时间单位（根据采样时间长度计算），打印当前的时间，用于显示进度。
disp( fix(t) ); % 显示进度
end
end
% 在这段代码中，使用一个循环来进行惯性导航的仿真步骤：
% 1. 在每次循环迭代开始时，通过增加nts来更新当前的时间t。
% 2. 调用insupdate函数来更新姿态（四元数qnb）、速度（vn）和位置（pos）。insupdate函数接受加速度（wm）和角速度（vm）的输入，以及时间步长ts。
vb% 在此步骤中，高度（vn(3)）被设为0，表示垂直方向上的速度保持不变。
% 3. 使用q2att函数将四元数qnb转换为姿态角（欧拉角）表示，得到姿态角（att）。
% 4. 将姿态角、速度、位置和时间存储到avp矩阵的第kk行。其中kk是一个计数变量，用于递增存储的行数。
% 5. 如果当前时间t模500的余数小于nts，表示经过了500个时间单位（根据采样时间长度计算），则显示当前的时间。这用于显示仿真进度。
% 6. 循环继续进行，直到达到预定的仿真步数len。
avp(kk:end, :) = []; % 18000行之后设置为空 为了保持avp矩阵的大小与实际仿真时长相匹配，将超出仿真时长部分的数据删除。
tt = avp(:, end); % tt为avp最后一列， 将时间信息提取出来存储在变量tt中。
mysubplot(221, tt, avp(:, 1:2) /arcdeg, '\theta, \gamma /\circ');
mysubplot(222, tt, avp(:, 3) /arcdeg, '\psi /\circ');
mysubplot(223, tt, avp(:, 4:6), 'v^n / m/s');
mysubplot(224, tt, deltapos(avp(:, 7:9)), '\Deltap / m');
%全局变量(glvs)
global GM Re ff wie ge gp ug arcdeg arcmin arcsec hur dph dps dpsh ugpsHz %定义全局变量
GM=3.986004415e14; %引力
Re=6.378136998405e6; %赤道长半轴
wie= 7.2921151467e-5; %自传角速度 wgs-84 model
ff=1/298.257223563; %扁率
ee=sqrt(2*ff-ff^2); %扁心率
e2=ee^2;
Rp=(1-ff)*Re; %极轴半径
ge=9.780325333434361; %ge 赤道重力加速度
gp=9.832184935381024; %gp 极点重力加速度
g0=9.780325333434361;
ug=ge*1e-6; %ug 微重力加速度
arcdeg=pi/180; %弧度=角度×π÷180° acdeg为弧度
arcmin=arcdeg/60; %弧分=弧度/60
arcsec=arcmin/60; %弧秒=弧度/60
hur=3600;
dph=arcdeg/hur;
dps = pi/180/1; % arcdeg / second
dpsh=arcdeg/sqrt(hur); %hour;deg/hour;deg/sqrt(hour) sqrt-开平方根
ugpsHz= ug/sqrt(1); %ug/sqrt(Hz)
%捷联惯导更新算法 参考4.1节
%eth.gcc 有害加速度
function [qnb,vn,pos,eth ]=insupdate(qnb,vn,pos,wm,vm,ts)
nn=size(wm,1); nts=nn*ts;
%进行圆锥误差/划船误差补偿。补偿后旋转矢量phim,补偿后速度增量dvbm 补偿圆锥误差和划船误差可以提高惯性导航系统的精度和稳定性，减小姿态估计的漂移，并提高导航解算的准确性
[phim,dvbm]=cnscl(wm,vm);
eth=earth(pos,vn); %地球相关参数计算
vn1=vn + rv2m( -eth.wnin*nts/2 )* qmulv( qnb,dvbm )+eth.gcc*nts;%速度更新
vn=(vn+vn1)/2;
%首先，使用旋转矩阵函数rv2m将地球自转速度eth.wnin乘以时间间隔的一半nts/2，得到一个旋转矩阵。
%然后，将旋转矩阵与姿态四元数qnb和速度增量dvbm进行乘积，使用四元数乘法函数qmulv，得到旋转后的速度增量。
%将地球重力加速度eth.gcc乘以时间间隔nts，与旋转后的速度增量相加，得到速度的更新量vn1。
%最后，将当前速度vn与更新量vn1相加，再除以2，得到速度的平均值，更新速度vn。
%实际上是将旋转导航系下的速度增量转换到机体系下，并考虑了地球自转的影响
pos=pos+[vn(2)/eth.RMh;vn(1)/eth.clRNh;vn(3) ]*nts;
vn=vn1; %位置更新
%首先，将当前速度的北向分量vn(2)除以地球的子午线半径eth.RMh，将当前速度的东向分量vn(1)除以地球的曲率半径eth.clRNh，得到一个长度为3的列向量。
%将上述列向量乘以时间间隔nts，得到一个长度为3的位置增量。
%将当前位置pos与位置增量相加，得到新的位置。
%考虑了地球的曲率半径和子午线半径对纬度和经度的影响，通过将速度在北向和东向的分量转换为对应的位置增量，实现了位置的更新。
%最终得到的新的位置pos包括更新后的纬度、经度和高度信息。
qnb=qmul( rv2q(-eth.wnin * nts), qmul(qnb, rv2q(phim)) ); %姿态更新
qnb=qnormlz(qnb);
%首先，根据角速度增量phim构造旋转矢量形式的四元数rv2q(phim)，表示机体系相对于导航系的旋转。
%将当前姿态四元数qnb与上述旋转矢量形式的四元数相乘，得到一个新的四元数。
%接着，根据导航系到机体系的旋转速度eth.wnin乘以时间间隔nts构造旋转矢量形式的四元数rv2q(-eth.wnin * nts)，表示导航系相对于机体系的旋转。
%将上述旋转矢量形式的四元数与新的四元数相乘，得到更新后的姿态四元数。
%最后，对更新后的姿态四元数进行归一化，确保其仍为单位四元数。
%反对称阵,斜对称阵 反对称矩阵常用于描述向量叉乘运算和旋转运动的代数表达式
function m=askew(v)
m=[ 0, -v(3), v(2);
v(3), 0, -v(1);
-v(2), v(1), 0 ];
% 生成反对称矩阵（斜对称矩阵）在数学和物理学中有许多应用，特别是在向量叉乘和旋转运动的描述中。
% 1. 叉乘运算：反对称矩阵可以用于表示向量的叉乘运算。给定两个向量 a 和 b，它们的叉乘结果可以表示为矩阵乘法的形式：a × b = M * b，其中 M 是由向量 a 生成的反对称矩阵。
% 2. 旋转运动：在刚体的旋转运动描述中，角速度向量与角位移向量之间存在线性关系。通过使用反对称矩阵，可以将角速度向量与角位移向量进行关联，从而方便地描述刚体的旋转。
% 3. 动力学建模：在物体的动力学建模中，反对称矩阵可以表示物体的旋转惯量矩阵，用于描述物体绕不同轴的转动惯量。
% 4. 控制系统：在控制系统中，反对称矩阵可以用于描述旋转运动的误差或补偿项，用于控制器的设计和姿态估计。
% 通过生成反对称矩阵，可以将向量运算和旋转运动的数学表达更简洁地表示出来，并方便在相关领域的计算和分析中使用。
%地球导航参数计算 参考3.1和3.2节
function eth=earth(pos,vn)
global Re ff wie ge %赤道长半轴 ；扁率 ；地球自转角速度；赤道重力加速度
ee=sqrt(2*ff-ff^2); e2=ee^2; %第一偏心率
eth.sl=sin(pos(1)); %sinL
eth.cl=cos(pos(1)); %cosL
eth.tl=eth.sl/eth.cl; %tanL
eth.sl2=eth.sl*eth.sl; eth.sl4=eth.sl2*eth.sl2; %(sinL)^2;(sin2L)^2
eth.sq=1-e2*eth.sl2;
eth.sq2=sqrt(eth.sq);
eth.RMh=Re*(1-e2)/eth.sq/eth.sq2; %子午圈主曲率半径+飞行高度 （3.1.23）
eth.RNh=Re/eth.sq2; %卯酉圈主曲率半径+飞行高度 （3.1.24）
eth.clRNh=eth.cl*eth.RNh;
eth.wnie=wie.*[0;eth.cl;eth.sl]; %地球自转角速度三个分量值x y z
eth.vn=vn; %载体运行的速度等地球速度，作为已知量
eth.wnen=[-vn(2)/eth.RMh;vn(1)/eth.RNh;vn(1)/eth.RNh*eth.tl]; %载体运动的角速度分量，设定东“+”西“-”。 （3.1.24）
%eth.wnen=[vn(2)/eth.RMh;-vn(1)/eth.RNh;-vn(1)/eth.RNh*eth.tl]; %西为“+”，东为“-”
eth.wnin=eth.wnie+eth.wnen; %载体相对惯性系的角速度
eth.wnien=eth.wnie+eth.wnin; %由比力方程推导的公式，2wie+wen
eth.gLh=ge*(1+5.27094e-3*eth.sl2+2.32718e-5*eth.sl4)-3.086e-6; %grs80重力模型 (3.2.23) %g0=ge;考虑高度不变，不加pos(3)
eth.gn=[0;0;-eth.gLh]; %注意方向垂直向下为“-”
%eth.gn=[0 ; 0 ;-9.7803];
eth.gcc = eth.gn - cross(eth.wnien , vn); %考虑重力/哥氏力/向心力
%旋转矢量转换为变换四元数 p20. (式2.4.21)
function q=rv2q(rv)
nm2=rv'*rv;
if nm2<1.e-8
q0=1-nm2*(1/8-nm2/384);s=0.5-nm2*(1/48-nm2/3840);
else
nm=sqrt(nm2);
q0=cos(nm/2);s=sin(nm/2)/nm;
end
q=[ q0;
s*rv
];
% 这段代码是用于将旋转矢量转换为变换四元数的函数。
% 1. 首先，计算旋转矢量的模长的平方，即 `nm2 = rv' * rv`。
% 2. 接下来，通过检查 `nm2` 是否小于一个很小的阈值（1.e-8），判断旋转矢量是否接近于零。
% 如果是，则采用近似的方式计算四元数的分量，以避免除以零的错误。具体计算如下：
% - `q0 = 1 - nm2 * (1/8 - nm2/384)`
% - `s = 0.5 - nm2 * (1/48 - nm2/3840)`
% 3. 如果旋转矢量不接近于零，则计算旋转矢量的模长 `nm`，然后计算四元数的分量，如下：
% - `q0 = cos(nm/2)`
% - `s = sin(nm/2) / nm`
% 4. 最后，将计算得到的四元数的分量 `[q0; s*rv]` 返回作为输出。
% 这段代码实现了旋转矢量到变换四元数的转换，考虑了旋转矢量接近零的特殊情况，并进行了适当的数值计算，以确保计算的准确性和稳定性。
%旋转矢量转换为变换矩阵 参考p11 （式2.2.23）
function m =rv2m(rv)
nm2=rv'* rv; %等效旋转矢量Φ的模方，'表示复共轭转置*原矩阵
if nm2<1.e-8 %如果模方很小，则可用泰勒展开前几项求三角函数
a= 1- nm2 * (1/6- nm2/120);b=0.5-nm2*(1/24-nm2/720);
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; %eye(3) 为3*3的单位矩阵I mk
% 该函数用于将旋转矢量转换为旋转矩阵。
% 首先，计算旋转矢量的模长的平方 `nm2`，即 `rv' * rv`，其中 `'` 表示复共轭转置，`*` 表示矩阵乘法。
% 然后，检查 `nm2` 是否小于一个很小的阈值（1.e-8）。如果是，则使用泰勒级数展开的近似方法来计算旋转矩阵的分量。
% - 计算 `a`：`a = 1 - nm2 * (1/6 - nm2/120)`
% 这个公式利用了泰勒级数展开的近似方法，通过二阶项近似计算 `a` 的值。它考虑了旋转矢量模长的平方 `nm2`，并根据该值进行近似计算。
% - 计算 `b`：`b = 0.5 - nm2 * (1/24 - nm2/720)`
% 同样利用了泰勒级数展开的近似方法，通过四阶项近似计算 `b` 的值。它也考虑了旋转矢量模长的平方 `nm2`，并根据该值进行近似计算。
% 接下来，使用旋转矢量的反对称矩阵 `VX` 来构建旋转矩阵。反对称矩阵的构造方式是将旋转矢量的分量放在矩阵的适当位置。
% 最后，计算旋转矩阵 `m`，它由单位矩阵和旋转矢量的反对称矩阵乘以系数 `a` 和 `b` 组成。
% 总结而言，该函数根据旋转矢量的模长的大小选择不同的计算方式，以获取旋转矩阵的近似值，以便将旋转矢量转换为旋转矩阵。
%由计算四元数和真实四元数计算失准角误差
function phi=qq2phi(qpb,qnb)
qerr=qmul(qnb,qconj(qpb));
phi=q2rv(qerr);
% 该函数用于计算两个四元数之间的姿态差，将其表示为旋转矢量（旋转矢量是描述旋转操作的向量）。
% 具体来说，输入参数 `qpb` 是由机体系到导航系的参考四元数，表示参考姿态；输入参数 `qnb` 是由机体系到导航系的实际四元数，表示实际姿态。
% 函数首先计算了两个四元数之间的差值，即姿态误差四元数 `qerr`。然后，使用 `q2rv` 函数将姿态误差四元数转换为旋转矢量 `phi`。
% 旋转矢量 `phi` 描述了从参考姿态到实际姿态的旋转操作。它可以用来衡量两个姿态之间的差异，并用于姿态控制、姿态估计等应用中。
% 需要注意的是，该函数的实现依赖于其他函数 `qmul`、`qconj` 和 `q2rv`，它们用于四元数的乘法、共轭和转换。
%四元数归一化
function qnb = qnormlz(qnb)
nm=qnb'*qnb; %共轭转置求模值
if nm<1e-6, qnb=[1;0;0;0]; %表示姿态的四元数，其模值应约为1
else
qnb=qnb/sqrt(nm);
end
% 该函数用于归一化表示姿态的四元数 `qnb`。
% 在姿态表示中，四元数通常被用来表示旋转姿态。为了确保四元数的有效性，即其模值接近1，需要对四元数进行归一化。归一化后的四元数可以更好地表示旋转姿态，并在姿态运算中提供更准确的结果。
% 函数中的操作是先计算四元数 `qnb` 的模值 `nm`（通过将四元数和其共轭转置相乘再求和得到）。
% 如果模值 `nm` 小于一个很小的阈值（1e-6），则将四元数设为 `[1;0;0;0]`，表示无旋转的姿态。否则，将四元数除以其模值的平方根，从而将其归一化为单位四元数。
% 归一化后的四元数具有单位模值，可以更好地用于姿态表示和姿态运算。
%四元数乘向量 参考2.4.26
function vo=qmulv(q,vi)
qi=[0;vi];
qo=qmul(qmul(q,qi),qconj(q));
vo=qo(2:4,1);
%vo=q2mat(q)*vi;
% 这个函数实现了四元数 `q` 与三维向量 `vi` 的乘法操作，将其命名为 `qmulv`。
% 在函数中，首先将向量 `vi` 在第一个分量上添加一个零元素，得到一个四元数形式的向量 `qi`。
% 然后，将 `q` 与 `qi` 进行两次乘法运算，分别是 `qmul(q, qi)` 和 `qmul(result, qconj(q))`。这样得到的结果是一个四元数形式的向量 `qo`。
% 最后，从 `qo` 中提取出后三个分量，形成一个新的三维向量 `vo`，即 `vo = qo(2:4, 1)`。
% 这样，函数实现了将四元数与向量相乘的操作，并返回乘积结果的三维向量形式。
% 注释中的另一行代码 `vo=q2mat(q)*vi` 是一种等效的实现方式，通过将四元数转换为旋转矩阵，再将旋转矩阵与向量相乘来实现相同的效果。
% 使用四元数来表示旋转的优势：
% 紧凑性：四元数只需要四个实数分量来表示旋转，而旋转矩阵需要九个实数分量。因此，四元数更加紧凑，占用更少的存储空间。
% 计算效率：四元数的乘法运算比旋转矩阵的乘法运算更加高效。四元数乘法可以通过简单的数学运算实现，而旋转矩阵的乘法涉及更复杂的矩阵乘法运算。
% 插值和平滑：在动画和插值算法中，使用四元数可以更容易地实现平滑的旋转过渡。四元数插值技术（如球面线性插值）可以确保旋转过渡的平滑性和连续性。
% 数学性质：四元数具有一些有用的数学性质，例如单位化约束和对偶性质，使得处理旋转操作更加方便。
%四元数相乘 参考p18 (式2.4.6)
function q =qmul(q1,q2)
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) ];
% 在惯性导航算法中，四元数相乘起到了表示姿态变换的作用。姿态是描述飞行器或导航系统在空间中的朝向或方向的参数。
% 而四元数是一种用于表示姿态的数学工具，它可以通过乘法运算来实现姿态的组合和变换。
% 具体来说，惯性导航算法中常用的姿态表示是由两个四元数组成的：qnb和qbn。
% 其中，qnb表示机体系（Body Frame）相对于导航系（Navigation Frame）的姿态，而qbn表示导航系相对于机体系的姿态。这两个四元数通过相乘运算来实现姿态的变换。
% 在惯性导航算法中，传感器提供的陀螺仪和加速度计等测量数据被用来更新姿态估计。通过将测量数据与当前姿态的四元数进行组合，可以得到新的姿态四元数，从而实现姿态的更新。
% 四元数的乘法运算在惯性导航算法中起到了关键的作用，它使得姿态的变换和更新成为可能，从而实现导航系统对飞行器的姿态跟踪和姿态控制。
%四元数减失准角误差
%失准角误差是由陀螺仪引起的，而陀螺仪的输出与姿态相关。因此，通过从姿态四元数中减去失准角误差，可以修正姿态估计中的偏差，使其更接近真实的姿态。
function qnb=qdelphi(qpb,phi)
qnb=qmul(rv2q(phi),qpb);
% %这段代码是将姿态误差（用旋转矢量表示）应用于姿态四元数的更新操作。具体而言，它使用了`rv2q(phi)`将旋转矢量`phi`转换为等效的旋转四元数。
% 然后，通过将旋转四元数与原始的姿态四元数`qpb`相乘，得到了考虑姿态误差的新的姿态四元数`qnb`。
% 该操作可以用于惯性导航算法中的姿态更新过程。姿态误差通常由陀螺仪的漂移等因素引起，需要通过传感器测量或其他方法进行补偿。
% 这段代码实现了将姿态误差转换为四元数形式，并将其应用于原始姿态四元数，从而得到更准确的姿态估计。
% 需要注意的是，四元数的乘法操作（`qmul`函数）在这里起到了将旋转矢量转换为四元数的作用。通过将旋转矢量转换为等效的旋转四元数，可以更方便地进行姿态更新和姿态组合的操作。
%四元数共轭 参考p13, 式2.4.13
function qout=qconj(qin)
qout=[ qin(1); -qin(2:4) ];
% 函数`qconj(qin)`用于计算给定四元数的共轭四元数。共轭四元数是指将原始四元数的虚部取负的结果，而实部保持不变。
% 具体而言，给定一个四元数qin，它可以表示为qin = [q0; q1; q2; q3]，其中q0是实部，q1、q2、q3是虚部。那么共轭四元数qout可以表示为qout = [q0; -q1; -q2; -q3]。
% 在惯性导航算法中，常常需要使用四元数的共轭来进行姿态变换或误差补偿。共轭四元数的应用可以帮助消除姿态估计中的误差或进行坐标系之间的变换。
% 函数`qconj(qin)`的作用就是根据给定的四元数qin计算其共轭四元数qout，并将其作为函数的输出。
%四元数加失准角误差
%失准角误差通常是由于传感器的非理想性、安装误差或其他环境因素引起的。通过将失准角误差加到四元数上，可以在姿态估计过程中对误差进行补偿，提高姿态的准确性和稳定性。
%失准角误差通常表示为旋转矢量或欧拉角的形式。在将失准角误差加到四元数上时，需要将失准角转换为四元数表示。这可以通过将失准角转换为旋转矩阵或旋转矢量，然后将旋转矩阵或旋转矢量转换为四元数实现。
function qpb=qaddphi(qnb,phi)
qpb=qmul(rv2q(-phi),qnb); %姿态转换矩阵*qnb
% 函数`qaddphi(qnb,phi)`实现了将失准角误差（表示为旋转矢量或欧拉角）加到姿态四元数`qnb`上的操作。
% 具体而言，它将失准角误差转换为四元数表示（通过将其转换为旋转矢量或欧拉角，并将其转换为四元数），然后将该四元数与姿态四元数相乘，得到修正后的姿态四元数`qpb`。
% 首先，失准角误差通过`rv2q(-phi)`转换为四元数表示，其中`-phi`表示失准角误差的相反方向。这里使用了函数`rv2q`将旋转矢量（或欧拉角）转换为四元数。
% 然后，使用`qmul(rv2q(-phi),qnb)`将失准角误差的四元数表示与姿态四元数`qnb`相乘。这里使用了函数`qmul`实现四元数的乘法操作，它将两个四元数相乘得到修正后的姿态四元数`qpb`。
% 通过将失准角误差加到姿态四元数上，可以对姿态进行修正，以减小误差对导航算法的影响，提高姿态估计的准确性和稳定性。
%变换四元数转换为旋转矢量
%首先，将四元数转化为标量非负的四元数；其次根据公式Q=q0+qv=cos(φ/2)+usin(φ/2),先由四元数的标量关系q0=cos(φ/2)求反函数，
%旋转矢量模值的一半(φ/2)=arccos(q0),再由矢量关系qv=(φ/2)*sin(φ/2)/(φ/2)求等效旋转矢量φ=2*((φ/2)/sin(φ/2))*qv.
function rv=q2rv(q)
if q(1)<0 , q=-q ;
end
nmhalf= acos(q(1)); %等效旋转矢量模值的一半
if nmhalf>1e-20 , b=2*nmhalf/sin(nmhalf);
else
b=2;
end
rv =b*q(2:4);
% %q是矩阵
% q=[ q0;
% s*rv
% ]; q(2:4)指第2至4个元素。
% % 这段代码实现了从四元数到旋转矢量的转换。具体解释如下：
% % 1. 首先，判断四元数的第一个元素q(1)是否小于0。如果小于0，则将四元数取负，以确保旋转矢量的模值非负。
% % 2. 接下来，计算等效旋转矢量模值的一半，即acos(q(1))。
% % 3. 如果等效旋转矢量模值的一半大于1e-20（一个很小的阈值），则计算缩放因子b，其值为2*nmhalf/sin(nmhalf)。这是为了将旋转矢量归一化。
% % 4. 如果等效旋转矢量模值的一半小于等于1e-20，则将缩放因子b设置为2。这是为了避免除以一个很小的数导致数值不稳定。
% % 5. 最后，将缩放因子b乘以四元数的第2至第4个元素，得到旋转矢量rv。注意，q(2:4)指的是四元数的第2至第4个元素。
% % 这个函数的作用是将四元数表示的姿态转换为等效的旋转矢量表示，用于某些计算或算法中。
%四元数转换为姿态阵 ，p246, B.8
% qnb=[q0;
% q1;
% q2;
% q3 ];
function Cnb=q2mat(qnb)
q11=qnb(1)*qnb(1);q12=qnb(1)*qnb(2);q13=qnb(1)*qnb(3);q14=qnb(1)*qnb(4);
q22=qnb(2)*qnb(2);q23=qnb(2)*qnb(3);q24=qnb(2)*qnb(4);
q33=qnb(3)*qnb(3);q34=qnb(3)*qnb(4);
q44=qnb(4)*qnb(4);
Cnb=[ q11+q22-q33-q44, 2*(q23-q14), 2*(q24+q13);
2*(q23+q24), q11-q22+q33-q44, 2*(q34-q12);
2*(q24-q13), 2*(q34+q12), q11-q22-q33+q44 ];
% 这段代码实现了将四元数转换为方向余弦矩阵（Direction Cosine Matrix，简称DCM）。DCM是描述旋转变换的一种常用方法，它表示了从一个坐标系到另一个坐标系的旋转关系。
% 具体来说，代码中的qnb是表示从导航坐标系到机体坐标系的旋转四元数。通过计算四元数的元素，可以得到方向余弦矩阵Cnb。Cnb是一个3x3的矩阵，其中的元素表示了导航坐标系与机体坐标系之间的旋转关系。
% 代码中的计算公式是根据四元数与方向余弦矩阵之间的关系推导而来。通过将四元数的元素代入公式，可以得到对应的方向余弦矩阵的元素。最终，将这些元素组合成一个3x3的矩阵Cnb返回。
% 注意，这里的代码实现的是一种特定的四元数到方向余弦矩阵的转换方法，具体的表示方式可能存在差异。在不同的应用中，可能会使用不同的表示方法或约定，所以需要根据具体的情况来确定使用何种转换方法。
%四元数转换为姿态角
function att=q2att(qnb)
%四元数->姿态阵->姿态角
att=m2att(q2mat(qnb));
% 函数`q2att(qnb)`的作用是将四元数转换为姿态角。具体的转换过程如下：
% 1. 调用函数`q2mat(qnb)`将四元数`qnb`转换为姿态阵（方向余弦矩阵）。
% 2. 调用函数`m2att()`将姿态阵转换为姿态角。
% 3. 返回姿态角。
% 这个函数的目的是将四元数表示的姿态信息转换为欧拉角或其他形式的姿态角表示，以便于理解和使用。
% 姿态角通常包括滚转角、俯仰角和偏航角，用于描述物体的旋转姿态。通过四元数到姿态阵再到姿态角的转换，可以得到更直观和可理解的姿态信息。
%姿态阵转为四元数 p247 . B.11
function qnb =m2qua(Cnb)
C11=Cnb(1,1);C12=Cnb(1,2);C13=Cnb(1,3);
C21=Cnb(2,1);C22=Cnb(2,2);C23=Cnb(2,3);
C31=Cnb(3,1);C32=Cnb(3,2);C33=Cnb(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
qnb=[q0;
q1;
q2;
q3 ];
% 函数`m2qua(Cnb)`的作用是将姿态阵（方向余弦矩阵）转换为四元数表示的姿态。具体的转换过程如下：
% 1. 根据姿态阵的元素进行判断，确定四元数的分量。
% 2. 根据不同的情况，计算四元数的分量`q0`、`q1`、`q2`、`q3`。
% 3. 将计算得到的四元数分量存储在向量`qnb`中。
% 4. 返回四元数`qnb`，表示姿态的四元数。
% 这个函数的目的是将方向余弦矩阵表示的姿态信息转换为四元数表示，以便于在惯性导航算法中使用。
% 四元数是一种紧凑且有效地表示姿态的方法，能够避免万向锁等问题，并且在旋转运算中具有良好的性质。通过姿态阵到四元数的转换，可以方便地进行姿态的更新和计算。
%姿态阵转换为姿态角 参考p245, B.4-B.7
function att=m2att(Cnb)
%当俯仰角在+-pi/2附近时，横滚角和航向角之间是无法单独分离的，不能计算出来。
if abs(Cnb(3,2))<=0.999999
att=[asin(Cnb(3,2)); -atan2(Cnb(3,1),Cnb(3,3)); -atan2(Cnb(1,2),Cnb(2,2)) ];
else
att=[asin(Cnb(3,2)); atan2(Cnb(1,3), Cnb(1,1)); 0 ];
end
% 函数`m2att(Cnb)`的作用是将姿态阵（方向余弦矩阵）转换为姿态角表示的姿态。具体的转换过程如下：
% 1. 首先判断姿态阵的元素`Cnb(3,2)`的绝对值是否小于等于0.999999，判断的目的是检查俯仰角是否在±π/2附近。如果不满足这个条件，则无法准确计算出横滚角和航向角。
% 2. 根据不同的情况，计算姿态角。
% - 如果满足条件，计算俯仰角为反正弦值`asin(Cnb(3,2))`，横滚角为反正切值`-atan2(Cnb(3,1),Cnb(3,3))`，航向角为反正切值`-atan2(Cnb(1,2),Cnb(2,2))`。
% - 如果不满足条件，计算俯仰角为反正弦值`asin(Cnb(3,2))`，横滚角为正切值`atan2(Cnb(1,3), Cnb(1,1))`，航向角设为0。
% 3. 将计算得到的姿态角存储在向量`att`中。
% 4. 返回姿态角`att`，表示姿态的俯仰角、横滚角和航向角。
% 这个函数的目的是将方向余弦矩阵表示的姿态信息转换为姿态角表示，以便于理解和使用。
% 姿态角表示的姿态信息更加直观和直接，能够方便地与实际场景进行对比和解释。然而，由于方向余弦矩阵存在一些限制和约束，所以在计算姿态角时需要注意特定的情况，以确保计算的准确性。
%姿态角转换为四元数，参考p247, B.12
function qnb =a2qua(att)
s=sin(att/2);c=cos(att/2);
si=s(1);sj=s(2);sk=s(3);
ci=c(1);cj=c(2);ck=c(3);
qnb=[ ci*cj*ck - si*sj*sk;
si*cj*ck - ci*sj*sk;
ci*sj*ck + si*cj*sk;
ci*cj*sk + si*sj*ck ];
%该转换也可通过姿态阵作为中间变量，先将姿态角转换为姿态阵再转换为四元数
%qnb=m2qua(a2mat(att));
% 这个函数实现了姿态角（欧拉角）到四元数的转换。姿态角是指描述刚体在空间中的姿态或方向的角度量，常用的姿态角包括横滚角（roll）、俯仰角（pitch）和航向角（yaw）。
% 在这个函数中，输入参数 `att` 是一个包含姿态角的向量，其中 `att(1)` 是横滚角，`att(2)` 是俯仰角，`att(3)` 是航向角。函数通过计算正弦和余弦值，将姿态角转换为对应的四元数。
% 具体的转换过程如下：
% 1. 计算姿态角的正弦和余弦值：`s = sin(att/2)` 和 `c = cos(att/2)`。
% 2. 将正弦和余弦值分别赋值给变量 `si`, `sj`, `sk`, `ci`, `cj`, `ck`。
% 3. 使用四元数的公式将姿态角转换为四元数：
% 最终的输出结果 `qnb` 是一个包含四元数的向量，表示姿态角对应的四元数表示。这个四元数可以用于进行姿态的计算和旋转运算。
%姿态角转换为姿态阵 参考p245, B.3式
function Cnb= a2mat(att)
s=sin(att);c=cos(att);
si=s(1);sj=s(2);sk=s(3);
ci=c(1);cj=c(2);ck=c(3);
% %输入姿态角向量att含三个分量，
% 依次为俯仰角θ(Theta)，横滚角γ(Goma)，航向角ψ(Fai)，
% 特别注意：程序中定位方位角北偏西为正，取值范围(-pi,pi].
%s(1)=sinθ,s(2)=sinγ,s(3)=sinψ；
%c(1)=cosθ,c(2)=cosγ,c(3)=cosψ；
Cnb=[ cj*ck-si*sj*sk, -ci*sk, sj*ck+si+cj*sk;
cj*sk+si*sj*sk, ci*ck, sj*ck-si*cj*ck;
-ci*sj, si, ci*cj ];
%地理坐标系（大地坐标系）转换为笛卡尔坐标系
function [x,y,z]=geo2xyz(B,L,h)
a=6378137;
f=1/298.257224;
b=a-a*f;
e=sqrt(a^2-b^2)/a;
N=a./(sqrt(1-e^2.*(sin(B)).^2));
x=(N+h).*cos(B).*cos(L);
y=(N+h).*cos(B).*sin(L);
z=(N*(1-e^2)+h).*sin(B);
end
% 该函数实现了地理坐标系（大地坐标系）转换为笛卡尔坐标系的转换过程。具体来说，该函数接受地理坐标系中的纬度（B）、经度（L）和高程（h），然后将其转换为笛卡尔坐标系中的 x、y 和 z 坐标。
% 函数中使用了以下参数：
% - a：地球长半轴（赤道半径），具体数值为 6378137 米。
% - f：地球扁率，具体数值为 1/298.257224。
% - b：地球短半轴（极半径），根据长半轴和扁率计算得到。
% - e：地球的第一偏心率，根据长半轴和短半轴计算得到。
% - N：曲率半径，在纬度 B 处的卯酉圈曲率半径，根据长半轴、短半轴和纬度计算得到。
% 根据大地测量学中的公式，通过以上参数和输入的 B、L 和 h 值，可以计算出 x、y 和 z 的值，表示相应的笛卡尔坐标系下的坐标。
% 需要注意的是，该函数假设地球是一个旋转椭球体，并采用了椭球体坐标系的数学模型。该模型在一定范围内提供了对地球形状的较好近似，适用于大多数地理测量和导航应用。
%辅助函数--求位置增量
function dpos=deltapos(pos) %delta为δ
cl=cos(pos(:,1)); %抽取pos中的第1列
Re=6378137;
% pos(1,1)代表第1行第1列的元数所在位置
dpos=[[(pos(:,1)-pos(1,1)),(pos(:,2)-pos(1,2)).*cl]*Re,pos(:,3)-pos(1,3)];
%matlab中的矩阵点乘(.*)和乘法(*) 。矩阵点乘：要求矩阵的行和列是完全相等的两个矩阵，对应元素相乘
% 该函数计算了给定位置向量 `pos` 相对于初始位置的位置增量（即偏移量） `dpos`。
% 具体来说，函数首先计算了纬度（`pos(:,1)`）的余弦值 `cl`，然后使用给定的地球半径 `Re` 对经度和纬度进行了计算。在计算经度时，将纬度的余弦值 `cl` 乘以经度的差值，以考虑纬度对经度的影响。
% 最后，将纬度、经度和高度的差值保存在 `dpos` 中。
% 这个函数可以用于计算航迹或轨迹中每个位置点相对于初始位置点的偏移量，以便进行导航或轨迹分析。
%圆锥/划船误差补偿 参考(2.6.24、4.1.55、2.6.3)
% %为了补偿该误差，通常采用多子样补偿算法，
% 在时间段T内进行N次采样，采样间隔为h=T/N,
% 每个采样间隔内的角增量称为子样
function [phim,dvbm]=cnscl(wm, vm) %phim/dvbm：经不可交换误差补偿后的等效旋转矢量/比力速度增量；wm为陀螺三轴角增量、vm为加表三轴速度增量
cs=[ [2, 0, 0, 0, 0 ]/3
[9, 27, 0, 0, 0 ]/20
[54, 92, 214, 0, 0 ]/105
[250, 525, 650, 1375, 0 ]/504
[2315, 4558, 7296, 7834, 15797 ]/4620 ]; %2-6子样补偿系数 p2.6.2 （表2.6.2 2.6.3）
wmm=sum(wm,1); vmm=sum(vm,1); %sum(A,1) 对 A 的列中的连续元素进行求和并返回一个包含每列之和的行向量。
dphim=zeros(1,3); %zeron（1,3）创建 由零组成的1*3向量。 （0,0,0）
scullm=zeros(1,3);
n=size(wm,1); %获取vm的行数,并赋值给n，为子样数
if n>1
csw=cs(n-1,1:n-1)*wm(1:n-1,:); csv =cs(n-1, 1: n-1)*vm(1:n-1 ,:);
dphim=cross(csw,wm(n,:)); %圆锥补偿量 %wm(n,:) 取第n行的所有元素
scullm=cross(csw,vm(n,:))+cross(csv,wm(n,:)); %划船补偿量
end
phim =(wmm+dphim)'; %圆锥补偿后的旋转矢量
dvbm =(vmm+0.5*cross(wmm,vmm)+scullm)'; %旋转和划船补偿后的速度增量
% 该函数用于对陀螺仪角增量和加速度计速度增量进行误差补偿，得到经过补偿后的等效旋转矢量 `phim` 和比力速度增量 `dvbm`。
% 具体来说，函数首先定义了一组补偿系数 `cs`，用于根据陀螺仪和加速度计的子样数对角增量进行补偿。然后，函数计算了陀螺仪角增量和加速度计速度增量的总和 `wmm` 和 `vmm`。
% 接下来，函数根据陀螺仪角增量的子样数选择合适的补偿系数，并使用这些系数对陀螺仪角增量和加速度计速度增量进行补偿。圆锥补偿量 `dphim` 使用圆锥补偿系数 `csw` 和陀螺仪角增量进行计算，
% 而划船补偿量 `scullm` 使用圆锥补偿系数 `csw` 和加速度计速度增量以及圆锥补偿系数 `csv` 和陀螺仪角增量进行计算。
% 最后，函数计算经过补偿后的等效旋转矢量 `phim` 和比力速度增量 `dvbm`。`phim` 的计算包括陀螺仪角增量总和 `wmm` 和圆锥补偿量 `dphim`，
% 而 `dvbm` 的计算包括加速度计速度增量总和 `vmm`、陀螺仪角增量和加速度计速度增量的交叉乘积，并加上划船补偿量 `scullm`。
% 该函数的目的是对陀螺仪和加速度计的测量误差进行补偿，以获得更准确的姿态和速度信息。
% 惯性传感器数据注入误差
% 通过向惯性传感器数据中注入一定的误差，可以更准确地模拟实际传感器的性能。这对于评估导航算法的鲁棒性、研究传感器融合技术以及开发和测试导航系统是非常有用的。
% 注入的误差可以包括固定偏差、随机游走误差、高频噪声等，以更真实地反映实际传感器的性能特征。
function [wm, vm] = imuadderr(wm, vm, eb, web, db, wdb, ts) % IMU添加零偏与游走误差
m = size(wm,1); % 获取行数
sts = sqrt(ts);
wm = wm + [ ts*eb(1) + sts*web(1)*randn(m,1), ...
ts*eb(2) + sts*web(2)*randn(m,1), ...
ts*eb(3) + sts*web(3)*randn(m,1) ];
vm = vm + [ ts*db(1) + sts*wdb(1)*randn(m,1), ...
ts*db(2) + sts*wdb(2)*randn(m,1), ...
ts*db(3) + sts*wdb(3)*randn(m,1) ];
end
% 这段代码的作用是给惯性测量单元（IMU）的角速度和加速度添加零偏和随机游走误差。
% 函数`imuadderr(wm, vm, eb, web, db, wdb, ts)`接受以下参数：
% - `wm`：角速度测量值的矩阵，大小为(m, 3)，每行代表一个时刻的角速度测量值。
% - `vm`：加速度测量值的矩阵，大小为(m, 3)，每行代表一个时刻的加速度测量值。
% - `eb`：角速度零偏的矢量，表示角速度测量值的固定偏差。
% - `web`：角速度随机游走误差的矢量，表示角速度测量值的随机变化。
% - `db`：加速度零偏的矢量，表示加速度测量值的固定偏差。
% - `wdb`：加速度随机游走误差的矢量，表示加速度测量值的随机变化。
% - `ts`：采样时间间隔。
% 函数的主要步骤如下：
% 1. 获取输入矩阵`wm`的行数m，表示角速度测量值的样本数量。
% 2. 计算采样时间间隔的平方根，保存为`sts`。
% 3. 对角速度测量值进行修正，将每个分量的零偏和随机游走误差添加到对应的测量值上。具体计算方式为：原始测量值加上时间间隔乘以零偏，再加上平方根时间间隔乘以随机游走误差乘以随机噪声。
% 4. 对加速度测量值进行修正，采用与角速度类似的方法。
% 5. 返回修正后的角速度矩阵`wm`和加速度矩阵`vm`。
% 这个函数模拟了IMU在测量角速度和加速度时的常见误差，包括固定偏差和随机游走误差。通过添加这些误差，可以更真实地模拟实际的IMU测量情况，从而更准确地进行姿态解算和导航算法的运算。
2935
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
