程序说明

概述

该程序实现了一个联邦滤波器的仿真，结合了惯性导航系统（SINS）、全球导航卫星系统（GNSS）和中央导航系统（CNS）的数据融合。程序的主要目的是比较中央卡尔曼滤波（CKF）与联邦卡尔曼滤波（FKF）的性能。

主要功能

• 轨迹设置：

定义采样间隔和初始位置、速度、姿态（AVP）。
使用 trjsegment 函数生成不同的运动轨迹段，包括匀速、加速、爬升及下降等。

• 传感器设置：

配置 GPS 和 CNS 模拟器，设定相应的误差。
生成包含姿态、速度、位置的 IMU 数据，并进行初始设置。

• 滤波器设置：

初始化中央和联邦卡尔曼滤波器的参数，包括状态转移矩阵、观测矩阵和噪声协方差矩阵。

• 滤波过程：

在主循环中，更新 IMU 数据并根据 GPS 和 CNS 数据进行滤波更新。
通过 kfupdate 和 fkfupdate 函数进行状态更新和反馈。

• 结果可视化：

绘制真实轨迹与滤波后轨迹的三维图。
生成位置误差图和累积分布函数图，分析不同滤波器的性能。

• 误差输出：

计算并打印出位置误差的最大值，便于性能评估。

代码结构

初始化部分：设置采样时间、初始状态和轨迹段。
传感器模拟：通过 gpssimu 和 cnssimu 函数生成传感器数据。
滤波器初始化：定义中央与联邦滤波器的参数。
主循环：处理数据更新和滤波计算，输出结果并进行可视化。

关键函数

trjsegment：用于生成运动轨迹的各段。
gpssimu 和 cnssimu：分别模拟 GPS 和 CNS 的输出。
kfupdate 和 fkfupdate：执行卡尔曼滤波更新。
imugpssyn：同步 IMU 和 GPS 数据。

运行结果与解析

三维轨迹：

三轴位置误差曲线：

程序代码

% 联邦滤波器159代码注释
% SINS GNSS CNS 159FKF
% Evand/2024-7-20/Ver1
clear;clc;close all;
rng(0);
glvs
%% 设置轨迹
ts = 0.1;       % sampling interval
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % init avp
% trajectory segment setting
traj_ = [];
seg = trjsegment(traj_, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, traj_, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'climb',        10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'descent',      10, 2, traj_, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  traj_, 2); %2
seg = trjsegment(seg, 'uniform',      100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);[nn, ts, nts] = nnts(2, trj.ts);
%% 传感器设置
% GPS simulator
fprintf('完整代码下载链接：https://mbd.pub/o/bread/Zpibm51p');
lever = [1; 2; 3]*0;