惯导与卫星组合导航仿真

影朔翔

于 2023-06-20 20:58:19 发布

阅读量1k

点赞数 4

文章标签： matlab

本文链接：https://blog.csdn.net/qq_67350186/article/details/131314707

版权

% 惯导与卫星组合导航仿真
clc
clear
% 惯性导航和卫星组合导航是常用的定位与导航技术，可以通过仿真来评估其性能。下面是惯性导航和卫星组合导航的简要仿真步骤：
% 1. 初始化导航系统：
% - 设置仿真时间和时间步长。
% - 初始化惯性传感器（陀螺仪和加速度计）和卫星导航系统（如GPS）。
% - 初始化导航滤波器，如扩展卡尔曼滤波器（EKF）或无迹卡尔曼滤波器（UKF）。
% 2. 生成传感器数据：
% - 根据设定的运动模型和环境条件，生成惯性传感器的模拟数据，包括角速度和加速度。
% - 根据设定的位置和卫星几何条件，生成卫星导航系统的模拟数据，包括卫星观测值。
% 3. 进行惯性导航：
% - 使用惯性传感器的数据进行姿态解算，例如利用陀螺仪数据进行姿态角增量积分。
% - 使用惯性传感器的数据进行速度和位置解算，例如利用加速度计数据进行速度和位移增量积分。
% 4. 进行卫星导航：
% - 使用卫星导航系统的观测数据进行位置和速度解算，例如利用GPS观测值进行定位。
% 5. 进行导航融合：
% - 将惯性导航和卫星导航的结果进行融合，例如使用EKF或UKF将两者的估计值进行融合。
% - 融合过程可以利用惯性导航的动态模型和卫星导航的测量模型进行状态估计和误差补偿。
% 6. 输出导航结果：
% - 输出融合后的位置和姿态估计结果。
% - 可选地，输出其他导航相关的信息，如速度、姿态角误差等。
% 7. 评估导航性能：
% - 根据真实的位置和姿态信息，计算导航结果与真实值的误差。
% - 分析导航误差的统计性质，如均方根误差（RMSE）或误差分布等。
% 通过以上仿真步骤，可以评估惯性导航和卫星组合导航系统在不同场景和条件下的性能，并优化导航算法和参数设置。注意，在实际应用中，还需要考虑传感器误差、环境扰动、滤波器设计等因素对导航性能的影响。

% glv; %加载全局变量
nn=2; %子样数，采用二样子圆锥误差补偿算法
ts=0.1; %单个采样间隔的采样时间长度
nts=nn*ts; %采样周期，在nts时间内进行两次采样，每次采样 0.1秒
%输入姿态角向量att含三个分量，依次为俯仰角θ(Theta)，横滚角γ(Gama)，航向角ψ(Fai)
att0=[1;1;30]*arcdeg; qnb0=a2qua(att0);
vn0=[0;0;0]; %初始化速度
%初始位置纬度（B）34.8222007度 ,经度（L）113.4845389度，高度（h）87.2566m B L H相当于大地坐标系的纬度、经度、高度
pos0=[34*arcdeg;108*arcdeg;100];
qnb=qnb0;vn=vn0;pos=pos0;% 姿态、速度和位置初始化；姿态角转换为四元数 b->n
%仿真静态IMU数据
eth=earth(pos,vn); %地球导航参数参数计算
%qconj 四元数共轭，地球自转角速度由导航系换算到机体系，作为机体角速度，并乘时间得到角度增量
wm=qmulv(qconj(qnb),eth.wnie)*ts;
%比力由导航系换算到机体系，作为机体比力，并乘时间得速度增量
vm=qmulv(qconj(qnb),-eth.gn)*ts;
wm=repmat(wm',nn,1); %repmat 重复数组副本。转置角速度矢量，并复制到nn行'1'列相当于二子样
vm=repmat(vm',nn,1); %转置速度矢量，并复制得到nn行'1'列
%仿真时间
phi=[0.1;0.2;3]*arcmin; %由计算四元数和真实四元数计算失准角误差 phi
qnb=qaddphi(qnb,phi); %四元数加失准角误差转换为qnb
%****至此与惯导仿真相同，以下进行注释****

eb = [0.01;0.015;0.02] * dph; % 陀螺随机常值漂移误差度/秒
web = [0.001;0.001;0.001] * dpsh; % 角度随机游走系数度/sqrt(秒)
db = [80;90;100] * ug; % 加速度计随机常值偏值误差 m/s^2
wdb = [1;1;1] * ugpsHz; % 速度随机游走系数
Qk = diag([web; wdb; zeros(9,1)])^2 * nts; % 系统噪声方差阵 15*15

rk = [[0.1;0.1;0.1];[[10;10]/Re;10]]; %观测噪声方差 6*1
Rk = diag(rk)^2; %观测噪声方差阵 6*6
% 初始化状态协方差矩阵
P0 = diag([[0.1;0.1;10]*arcdeg; ...
[1;1;1]; ...
[[10;10]/Re;10]; ...
[0.1;0.1;0.1]*dph; ...
[100;100;100]*ug])^2; % % 状态协方差矩阵15*15
% 初始化观测矩阵
Hk = [zeros(6,3),eye(6),zeros(6)]; % 6*15

kf = kfinit(Qk, Rk, P0, zeros(15), Hk); % 卡尔曼滤波器初始化系统噪声方差阵（Qk）观测噪声方差阵（Rk）初始状态协方差矩阵（P0）观测矩阵（Hk）
%通过使用卡尔曼滤波器对象，您可以在仿真过程中通过调用相应的滤波函数（如kfupdate）来执行滤波操作，并根据传感器测量值更新状态估计和状态协方差矩阵。

len = fix(3600/ts); % 仿真时长
err = zeros(len, 10); % 记录结果
xkpk = zeros(len, 2*kf.n+1); %用于保存卡尔曼滤波的状态估计和状态协方差。
kk = 1;
t = 0;

for k=1:nn:len
t = t + nts;
[wm1, vm1] = imuadderr(wm, vm, eb, web, db, wdb,ts);
[qnb, vn, pos, eth] = insupdate(qnb, vn, pos, wm1, vm1,ts);
kf.Phikk_1 = eye(15) + kfft15(eth,q2mat(qnb),sum(vm1,1)'/nts)*nts;
kf = kfupdate(kf);
% 在上述代码中，使用了一个循环来执行仿真过程。以下是每个循环迭代的具体步骤：
% 1. 更新时间：通过将当前时间 `t` 增加采样间隔 `nts`，更新仿真的当前时间。
% 2. 添加传感器误差：使用 `imuadderr` 函数添加陀螺仪和加速度计的误差。根据定义的误差值 `eb`、`web`、`db`、`wdb` 和采样时间间隔 `ts`，计算陀螺仪和加速度计的带有误差的测量值 `wm1` 和 `vm1`。
% 3. 更新惯导系统：通过调用 `insupdate` 函数，使用带有误差的测量值 `wm1` 和 `vm1` 更新导航系统的姿态、速度和位置信息。
% 得到更新后的四元数 `qnb`、速度矢量 `vn`、位置矢量 `pos` 和地球姿态 `eth`。
% 4. 更新卡尔曼滤波器的状态转移矩阵：根据导航系统的姿态和加速度计的测量值，更新卡尔曼滤波器的状态转移矩阵 `kf.Phikk_1`。
% 5. 执行卡尔曼滤波器更新：通过调用 `kfupdate` 函数，执行卡尔曼滤波器的状态更新和误差校正。

if mod(t,1)<nts % 整秒时执行下列操作
gps = [vn0; pos0] + rk.*randn(6,1); % GPS速度位置仿真,添加误差
kf = kfupdate(kf, [vn;pos]-gps, 'M'); % sins与gps差值作为量测值修正系统状态
vn(3) = vn(3) - kf.Xk(6);
kf.Xk(6) = 0;
end
% 在每个整秒时刻，根据上述代码执行以下操作：
% 1. 生成带有误差的GPS速度和位置测量值：使用 `rk`（定义了GPS速度和位置的误差大小）和随机数生成函数 `randn`，生成具有误差的GPS速度和位置测量值 `gps`。误差通过将随机数乘以 `rk` 来添加到真实值中。
% 2. 使用差值进行卡尔曼滤波器更新：调用 `kfupdate` 函数，将SINS与GPS测量值之间的差值作为量测值，通过修正系统状态来更新卡尔曼滤波器。
%具体而言，将 `[vn; pos] - gps` 作为量测值传递给 `kfupdate` 函数，并指定测量类型为 `'M'`（差值量测）。
% 3. 修正垂直速度：通过将卡尔曼滤波器状态向量 `kf.Xk` 中对应垂直速度的部分 `kf.Xk(6)` 从导航系统的垂直速度 `vn(3)` 中减去，并将卡尔曼滤波器状态向量中对应部分 `kf.Xk(6)` 设置为零，来修正垂直速度误差。
%通过修正系统状态来更新卡尔曼滤波器意味着根据测量值与系统预测之间的差异，调整卡尔曼滤波器的状态估计。这通常包括以下步骤：
% 1. 预测步骤：使用系统动力学模型和卡尔曼滤波器的先验状态估计，进行状态预测。这会生成卡尔曼滤波器的先验状态和协方差估计。
% 2. 量测步骤：将测量值与预测值进行比较，得到测量残差（测量值与预测值之间的差异）。在卡尔曼滤波器中，通常使用测量残差来构建一个观测矩阵（通常称为H矩阵），该矩阵将系统状态与测量值关联起来。
% 3. 修正步骤：通过将测量残差乘以卡尔曼增益，将其添加到先验状态估计中，从而修正系统的状态估计。卡尔曼增益反映了测量值与预测值之间的不确定性和可靠性。
% 通过修正系统状态来更新卡尔曼滤波器，可以融合测量值和系统预测，提高状态估计的准确性和稳定性。

% 记录
qnb=qdelphi(qnb,kf.Xk(1:3));kf.Xk(1:3)=0;%反馈
vn=vn-kf.Xk(4:6);kf.Xk(4:6)=0;
pos=pos-kf.Xk(7:9);kf.Xk(7:9)=0;
err(kk,:) = [qq2phi(qnb,qnb0); vn-vn0; pos-pos0; t]';
xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t];
kk = kk+1;
% 在代码中，通过反馈来更新状态估计和状态协方差矩阵，并将修正后的结果应用于系统的状态和协方差估计。
% 具体操作如下：
% - 首先，使用`qdelphi`函数将卡尔曼滤波器估计的姿态误差修正应用于导航系统的四元数姿态`qnb`，并将修正后的姿态误差置零。
% - 接下来，使用`kf.Xk(1:3)`的值减去导航速度`vn`，将修正后的速度误差应用于导航速度，并将修正后的速度误差置零。
% - 然后，使用`kf.Xk(7:9)`的值减去位置`pos`，将修正后的位置误差应用于导航位置，并将修正后的位置误差置零。
% - 将修正后的姿态误差、速度误差、位置误差和时间存储在`err`矩阵中，用于后续分析和显示。
% - 将卡尔曼滤波器的状态估计向量、协方差矩阵的对角线元素以及时间存储在`xkpk`矩阵中，用于后续分析和显示。
% - 最后，更新计数器`kk`，以便记录下一个时间步的结果。
% 通过反馈修正系统的状态估计，可以根据卡尔曼滤波器的输出，对导航系统的姿态、速度和位置进行修正，以获得更准确的导航解算结果。
% 显示进度
if mod(t,500)<nts
disp(fix(t));
end
end
err(kk:end,:) = [];
xkpk(kk:end,:) = [];
tt = err(:,end);
% 在上述代码中，通过使用`mod(t,500)<nts`的条件来判断是否达到了500个时间单位（根据采样时间长度计算）。
% 如果满足条件，表示已经过了500个时间单位，那么就会打印当前时间的整数部分（使用`fix(t)`函数），以显示仿真进度。
% 在循环结束后，对存储结果的`err`矩阵和`xkpk`矩阵进行修剪，将多余的行删除。这是因为循环可能不会完全覆盖整个矩阵，所以需要将多余的行删除。
% 最后，将`err`矩阵中的最后一列（时间列）存储到变量`tt`中，以便后续分析和显示。

% 状态真值与估计效果对比图
msplot (321, tt , err (:,1:2)/ arcmin ,'\ it \ phi \ rm /(\prime)');
legend ('\it\phi\rm_E ','\ it \ phi \ rm_N ');
msplot (322, tt , err (:,3)/ arcmin ,'\ it \ phi \ rm_U \ rm /(\ prime )');
msplot (323, tt , err (:,4:6),'\ delta \ itvn \ rm /( m .s {-1})');
legend ('\delta \ itv \ rm_E' ,'\ delta \ itv \ rm_N' ,'\ delta \ itv \ rm_U');
msplot (324, tt ,[ err(:,7)* Re , err(:,8)* Re * cos( pos (1)), err(:,9)],'\ delta \ itp \ rm / mr' );
legend ('\ delta \ itL' ,'\ delta \ it \ lambda '.'\ delta \ ithr' )
msplot (325,tt, xkpk(:,10:12/dph,'\ it \ epsilon \ rm /(\ circ.h^{-1})'));
legend ('\ it \ epsilon_x ','\ it \ epsilon_y' ,'\ it \ epsilon_z ')
msplot (326,tt , xkpk(:,13:15)/ ug ,'\ it \ nabla \ rm /\ mu \ itg');
legend ('\ it \ nabla_x' ,'\ it \ nabla_y','\ it \ nabla_z')
%均方差收敛图
spk = sqrt ( xkpk (:,16:end-1));
msplot (321, tt , spk (:,1:2)/ arcmin ,'\ it \ phi \ rm /(\ prime )');
legend ('\ it \ phi \ rm_E' ,'\ it \ phi \ rm_N' )
msplot (322,tt ,spk (:,3)/ arcmin ,'\ it \ phi \ rm_U \ rm /(\ prime )');
msplot (323, tt , spk (:,4:6),'\ delta \ itvn \ rm /( m .s {-1})');
legend ('\ delta \ itv \ rm _ E ','\ delta \ itv \ rm _ N' ,'\ delta \ itv \ rm _ U' )
msplot (324, tt ,[[ spk(:,7), spk(:,8)* pos (1)]* Re , spk(:,9)],'\ delta \ itp \ rm / m' );
legend ('\ delta \ itL ','\ delta \ it \ lambda ','\ delta \ ith' )
msplot (325,tt , spk(:,10:12)/ dph ,'\ it \ epsilon \ rm /(\ circ.h {-1})');
legend ('\ it \ epsilon _ x ','\ it \ epsilon _ y' ,'\ it \ epsilon _ z' )
msplot (326,tt, spk(:,13:15)/ ug ,'\ it \ nabla \ rm /\ mu \ itg' );
legend ('\ it \ nabla _ x ','\ it \ nabla _ y ','\ it \ nabla _ z' )

% 卡尔曼滤波更新
% 参考5.2节
% kf - 卡尔曼滤波结构体数组
% Zk - 量测向量
% TimeMeasBoth - 更新判断条件,
% TimeMeasBoth='T' (nargin==1) 仅根据时间更新系统状态
% TimeMeasBoth='M' 仅根据量测值更新系统状态
% TimeMeasBoth='B' (nargin==2) 根据时间和量测值更新系统状态

function kf = kfupdate(kf, Zk, TimeMeasBoth)

% 判断输入元素个数
if nargin==1 % 仅有kf输入,仅根据时间更新系统状态
TimeMeasBoth = 'T';
elseif nargin==2 % 两个输入,根据时间和量测值更新系统状态
TimeMeasBoth = 'B';
end

% 根据时间进行系统状态的更新
if TimeMeasBoth=='T' || TimeMeasBoth=='B'
kf.Xkk_1 = kf.Phikk_1*kf.Xk; % 参考(5.2-5)计算先验状态估计Xkk_1，使用状态转移矩阵Phikk_1乘以当前状态估计Xk。

kf.Pkk_1 = kf.Phikk_1*kf.Pk*kf.Phikk_1' + kf.Tauk*kf.Qk*kf.Tauk';

%计算先验协方差矩阵Pkk_1，使用状态转移矩阵Phikk_1乘以先验协方差矩阵Pk，再加上过程噪声协方差矩阵Qk和过程噪声转移矩阵Tauk的乘积。
% 参考(5.2-7)

else

kf.Xkk_1 = kf.Xk;
kf.Pkk_1 = kf.Pk;

% TimeMeasBoth=='M' 如果TimeMeasBoth为'M'，则直接使用当前状态估计Xk和协方差矩阵Pk作为先验状态估计Xkk_1和先验协方差矩阵Pkk_1。

end
% 该函数是一个卡尔曼滤波器的更新函数，用于根据时间和量测值更新系统的状态估计和协方差矩阵。
% 根据TimeMeasBoth的值判断需要执行的更新步骤：
% - 如果TimeMeasBoth为'M'或'B'，则根据量测值更新系统状态：
% - 计算测量协方差矩阵PZkk_1，使用观测模型矩阵Hk乘以先验协方差矩阵Pkk_1和观测模型矩阵Hk的转置，再加上观测噪声协方差矩阵Rk。
% - 计算卡尔曼增益Kk，使用先验协方差矩阵Pkk_1和测量协方差矩阵PZkk_1的乘积除以测量协方差矩阵PZkk_1。
% - 计算后验状态估计Xk，使用先验状态估计Xkk_1加上卡尔曼增益Kk乘以量测值Zk与观测模型矩阵Hk乘以先验状态估计Xkk_1的差。
% - 计算后验协方差矩阵Pk，使用先验协方差矩阵Pkk_1减去卡尔曼增益Kk乘以测量协方差矩阵PZkk_1和卡尔曼增益Kk的转置的乘积。
% 最后，对更新后的后验协方差矩阵Pk进行对称化处理，确保其为对称矩阵。
% 返回更新后的卡尔曼滤波器对象kf。

% 根据量测值对系统状态进行更新
if TimeMeasBoth=='M' || TimeMeasBoth=='B'
kf.PXZkk_1 = kf.Pkk_1*kf.Hk'; % 参考(5.2-13)计算先验状态估计与观测模型的协方差矩阵乘积
kf.PZkk_1 = kf.Hk*kf.PXZkk_1 + kf.Rk; % 结合(5.2-12)(5.2-13)计算观测模型与先验状态估计与观测模型的协方差矩阵乘积，并加上观测噪声协方差矩阵
kf.Kk = kf.PXZkk_1/kf.PZkk_1; % 参考(5.2-28)计算卡尔曼增益
kf.Xk = kf.Xkk_1 + kf.Kk*(Zk-kf.Hk*kf.Xkk_1);% 参考(5.2-15)更新后验状态估计，其中Zk是量测值。
kf.Pk = kf.Pkk_1 - kf.Kk*kf.PZkk_1*kf.Kk'; % 结合(5.2-12)(5.2-31b)更新后验协方差矩阵
else % TimeMeasBoth=='T'
kf.Xk = kf.Xkk_1;
kf.Pk = kf.Pkk_1;
%如果TimeMeasBoth的值为'T'，即仅根据时间更新系统状态，则直接将先验状态估计和先验协方差矩阵赋值给后验状态估计和后验协方差矩阵
end

kf.Pk = (kf.Pk+kf.Pk')/2; % 对后验协方差矩阵进行对称化处理，保证其为对称矩阵
end

% 卡尔曼滤波器初始化
function kf = kfinit(Qk, Rk, P0, Phikk_1, Hk, Tauk)
[kf.m, kf.n] = size(Hk);
kf.Qk = Qk;
kf.Rk = Rk;
kf.Pk = P0;
kf.Xk = zeros(kf.n,1);
kf.Phikk_1 = Phikk_1; % 零矩阵
kf.Hk = Hk;
% 没有输入系统噪声分配矩阵，默认为单位阵
if nargin < 6
kf.Tauk = eye(kf.n);
else
kf.Tauk = Tauk;
end
end
% 该函数用于初始化卡尔曼滤波器（Kalman Filter）对象。
% 输入参数：
% - `Qk`：过程噪声协方差矩阵
% - `Rk`：观测噪声协方差矩阵
% - `P0`：初始状态协方差矩阵
% - `Phikk_1`：状态转移矩阵的初始值（零矩阵）
% - `Hk`：观测模型矩阵
% - `Tauk`：状态转移矩阵的缩放因子（可选，默认为单位矩阵）
% 输出结果：
% - `kf`：初始化后的卡尔曼滤波器对象，包含以下属性：
% - `m`：观测向量的维度
% - `n`：状态向量的维度
% - `Qk`：过程噪声协方差矩阵
% - `Rk`：观测噪声协方差矩阵
% - `Pk`：后验状态协方差矩阵的初始值（由`P0`指定）
% - `Xk`：后验状态估计的初始值（全零向量）
% - `Phikk_1`：状态转移矩阵的初始值（由`Phikk_1`指定）
% - `Hk`：观测模型矩阵
% - `Tauk`：状态转移矩阵的缩放因子（由`Tauk`指定，默认为单位矩阵）
% 函数首先确定观测向量的维度和状态向量的维度，并将其保存在卡尔曼滤波器对象的属性`m`和`n`中。
% 然后，将输入的过程噪声协方差矩阵、观测噪声协方差矩阵、初始状态协方差矩阵分别赋值给卡尔曼滤波器对象的属性`Qk`、`Rk`和`Pk`。
% 将后验状态估计的初始值设置为全零向量，并将状态转移矩阵的初始值设为输入的`Phikk_1`（一般为零矩阵）。
% 将观测模型矩阵赋值给卡尔曼滤波器对象的属性`Hk`。
% 如果输入参数的个数小于6，则默认状态转移矩阵的缩放因子为单位矩阵；否则，将输入的`Tauk`赋值给卡尔曼滤波器对象的属性`Tauk`。
% 最后，返回初始化后的卡尔曼滤波器对象`kf`。

% SINS误差转移矩阵
% 参考4.2.5和6.3.3节及earth函数
function Ft = kfft15(eth, Cnb, fb)
global g0
tl = eth.tl;
secl = 1/eth.cl;
f_RMh = 1/eth.RMh;
f_RNh = 1/eth.RNh;
f_clRNh = 1/eth.clRNh;
f_RMh2 = f_RMh*f_RMh;
f_RNh2 = f_RNh*f_RNh;
vE_clRNh = eth.vn(1)*f_clRNh;
vE_RNh2 = eth.vn(1)*f_RNh2;
vN_RMh2 = eth.vn(2)*f_RMh2;
% 这段代码计算了一些中间变量，用于后续状态转移矩阵的计算：
% - `tl`：经度切换系数，从`eth.tl`中获取。
% - `secl`：纬度余切的倒数，即`1/eth.cl`。
% - `f_RMh`：子午线曲率半径的倒数，即`1/eth.RMh`。
% - `f_RNh`：卯酉圈曲率半径的倒数，即`1/eth.RNh`。
% - `f_clRNh`：纬度余切与卯酉圈曲率半径的乘积的倒数，即`1/eth.clRNh`。
% - `f_RMh2`：子午线曲率半径的倒数的平方，即`f_RMh*f_RMh`。
% - `f_RNh2`：卯酉圈曲率半径的倒数的平方，即`f_RNh*f_RNh`。
% - `vE_clRNh`：导航速度的东向分量与纬度余切与卯酉圈曲率半径的乘积的乘积，即`eth.vn(1)*f_clRNh`。
% - `vE_RNh2`：导航速度的东向分量与卯酉圈曲率半径的倒数的平方的乘积，即`eth.vn(1)*f_RNh2`。
% - `vN_RMh2`：导航速度的北向分量与子午线曲率半径的倒数的平方的乘积，即`eth.vn(2)*f_RMh2`。
Mp1 = [ 0, 0, 0;
-eth.wnie(3), 0, 0;
eth.wnie(2), 0, 0 ];
Mp2 = [ 0, 0, vN_RMh2;
0, 0, -vE_RNh2;
vE_clRNh*secl, 0, -vE_RNh2*tl];
Maa = askew(-eth.wnin);
Mav = [ 0, -f_RMh, 0;
f_RNh, 0, 0;
f_RNh*tl, 0, 0 ];
Map = Mp1 + Mp2;
Mva = askew(Cnb*fb);
Mvv = askew(eth.vn)*Mav - askew(eth.wnien);
Mvp = askew(eth.vn) * (Mp1+Map);
scl = eth.sl * eth.cl;
Mvp(3,1) = Mvp(3,1) - g0 *(5.27094e-3*2*scl+2.32718e-5*4*eth.sl2*scl);
Mvp(3,3) = Mvp(3,3) + 3.086e-6;
Mpv = [ 0, f_RMh, 0;
f_clRNh, 0, 0;
0, 0, 1 ];
Mpp = [ 0, 0, -vN_RMh2;
vE_clRNh*tl, 0, -vE_RNh2*secl;
0, 0, 0 ];
O33 = zeros(3);
% phi dvn dpos eb db
Ft = [ Maa Mav Map -Cnb O33
Mva Mvv Mvp O33 Cnb
O33 Mpv Mpp O33 O33
zeros(6,15) ];
end
% 这段代码计算了状态转移矩阵`Ft`的各个分块矩阵。下面是对各个矩阵的说明：
%Maa：与姿态角（phi）相关的矩阵。
%scl：eth.sl和eth.cl的乘积。
% Mav：与速度变化（dvn）和位置变化（dpos）相关的矩阵。
% Map：与陀螺仪漂移（eb）相关的矩阵。
% -Cnb：姿态误差矩阵的负值。
% Mva：与姿态角速度（wnin）和加速度计测量值（fb）相关的矩阵。
% Mvv：与速度变化相关的矩阵。
% Mvp：与速度变化和姿态误差矩阵相关的矩阵。
% Cnb：姿态误差矩阵。
% O33：一个3x3的零矩阵。
% Mpv：与位置变化和姿态误差矩阵相关的矩阵。
% Mpp：与位置变化相关的矩阵。
% - `Ft`：状态转移矩阵，由各个分块矩阵组成。这样构建的状态转移矩阵 Ft 用于描述系统状态在一个离散时间步长内的演化规律。