【PSINS】CNS/SINS滤波和GPS/SINS滤波的对比|matlab代码|速度对比、位置对比

PSINS

本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

• PSINS初学指导
• 基于PSINS的相关程序设计（付费专题）

两个滤波的不同点

CNS/SINS滤波

使用184的滤波结构：18维状态量、4为观测量
CNS探测角度（以恒星为观测）和高度（以地球为观测），滤波时观测量为角度（三维）+高度（一维）。

GPS/SINS滤波

使用183的滤波，观测量为GPS的三维位置。

滤波结果

运行程序，得到如下运行结果：
CNS观测的角度值：

CNS滤波值与真值的对比：

GPS滤波值与真值的对比：

下面是一些原创的对比图：
三维轨迹对比：

纬度经度高度对比（两个算法绘制在一起）：

滤波后X轴（纬度）位置累积概率分布函数：

累积分布函数（Cumulative Distribution Function，简称CDF）是一种描述随机变量取值的概率分布的函数。对于一个随机变量X，它的累积分布函数F(x)定义为X小于等于x的概率，即：
$$F(x)=P(X\le x)$$
累积分布函数可以用来计算和描述随机变量取值的概率。

例如，对于一个标准正态分布的随机变量X，它的累积分布函数可以表示为：
$$F(x)=P(X\le x)=\int [-\infty ,x](1/\surd (2\pi )\ast e^{(}-t^2/2))dt$$
其中e是自然对数的底，π是圆周率。这个累积分布函数可以用来计算X小于等于某个具体取值x的概率。

代码部分

代码由如下部分组成：

源代码如下：

% SINS/CNS与SINS/GPS的对比 184 EKF
% 2024-7-24/Ver1
clear;clc;close all;
rng(0);
glvs
psinstypedef('test_SINS_CNS_def');
ts = 0.1;       % sampling interval
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % 初始化avp
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);%% CNS simulation
mu = [1;2;5]*glv.min;
rk = [[10;10;30]*glv.sec;10];
[qis, utc0] = cnssimu(trj.avp, rk(1:3), mu, [2021;11;22;12*3600; -0.1;37]);
%% initial settings
[nn, ts, nts] = nnts(1, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, .5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.01, [1;1;3]);
ins = insinit(avpadderr(trj.avp0, davp0), ts);
fprintf('剩余代码下载链接如下：https://gf.bilibili.com/item/detail/1105873012');
%% KF filter
Cie0 = cnsCie(utc0(1:3), utc0(4),  utc0(5), utc0(6));