高精度定位定向系统后处理软件使用说明

图中，1-3列为陀螺角增量(rad)，4-6列为加速度计速度增量(m/s)，7列为IMU时间（s）;8-10列为卫导E/N/U速度（m/s），11-13列为卫导位置（纬/经度rad、高度m），14列为卫星有效性标志（0/1），15列为同步时间差（s，dt=IMUt-GNSSt）。如果文件某行没有卫星信号，则该行只有IMU数据有值，而卫导数据全填0，数据文件虽存在很多无用的0信息，但该数据对其方式方便C++处理。

画出IMU和GNSS原始数据图分别如下：

图2

图3

图中“*”为GNSS-RTK非固定解结果。

3.正反向滤波

使用VC6.0打开PSINS.dsw工程文件，并设置合适的原始数据“\data\imugps.bin”所在文件夹路径、以及输出处理结果保存路径和文件名，如下图：

图4

后处理程序主体包含初始对准->正向Kalman滤波->反向Kalman滤波及融合三个过程。通过图2的IMU数据查看，可选取前期静止时间段的数据做初始对准，对准开始时确保GNSS定位数据有效。

Kalman滤波状态选为34维，如下

状态中各符号（分量序号）依次为失准角（1-3）、速度误差（4-6）、位置误差（7-9）、陀螺漂移（10-12）、加计偏值（13-15）、IMU/GNSS杆臂（16-18）、IMU/GNSS不同步时差（19）、陀螺标定误差（20-28）、加计标定误差（29-34），分别定义如下

运行程序，窗口显示进度如下：

图5

后处理软件运行完成后，导航结果、滤波状态及均方误差信息默认存储文件为“\data\posres.bin”和“\data\fusion.bin”。

4.数据融合与结果查看

（1）Matlab融合

对正反向滤波结果进行融合，采用最简单的加权平均方法就行，可由C++的正反向输出文件posres.bin通过Matlab处理。使用‘\data\show_pos.m’中程序进行数据加载和融合，如下

n = 34;

res = binfile('posres.bin', 21+n*2+6+3); len2 = length(res)/2;

avp = res(:,[1:21,end]); % insplot(avp(:,[1:15,end]));

avp1 = avp(1:len2,:);

avp2 = flipud(avp(len2+1:end,:)); avp2(:,[4:6,10:12,19])=-avp2(:,[4:6,10:12,19]);

xk = res(:,[22:21+n,end]);

xk1 = xk(1:len2,:);

xk2 = flipud(xk(len2+1:end,:)); xk2(:,[4:6,10:12,19])=-xk2(:,[4:6,10:12,19]);

pk = res(:,[22+n:21+2*n,end]);

pk1 = pk(1:len2,:);

pk2 = flipud(pk(len2+1:end,:));

avpf = [fusion(avp1(:,1:15), pk1(:,1:15), avp2(:,1:15), pk2(:,1:15), 1), avp1(:,end)];

完成融合后，将GNSS（蓝色）、正向滤波（红色）、反向滤波（洋红）、融合（黄色）四条平面轨迹画图如下

gps1 = avp1(avp1(:,19)>0.1, [16:21,end]);

pos2dplot(gps1(:,4:7), avp1(:,[7:9,end]), avp2(:,[7:9,end]), avpf(:,[7:9,end]));

图6

从图中大部分的局部放大可以看出，黄色的融合结果总是在红色正向和洋红反向的中间，曲线更平滑，应该精度更高。

特别模拟700-1000s时段共300s的GNSS丢星，绘制正向、反向和融合三者与GNSS的位置误差，如下

avpcmpplot(gps1, avp1(:,[4:9,end]), avp2(:,[4:9,end]), avpf(:,[4:9,end]), 'vp');

图7

图中，点划线为正向Kalman滤波误差，长虚线为反向误差，短虚线为融合误差。多数情况下“正向误差偏左，反向误差偏右，正反融合恰好左右相互抵消”，因而融合误差会比较小。可能得注意丢星的两端点融合有一些特殊需要处理的(容易解决)。

量测噪声自适应结果：

rk = res(:, [22+2*n:21+2*n+6,end]); rvpplot(rk);

图8

状态估计结果：

inserrplot(xk, 'avpedltkgka')

图9

前15维状态进行了反馈校正，接近于零；而16-18维分量杆臂和19维分量时间不同步没有反馈校正，所以显示有值不为零。注意杆臂估计值于C++中故意设置“kf.lvGPS = CVect3(1.0, 0.5, 0.5)”接近，改成其它值也可大概估计出来；时间不同步约14ms，说明IMU与GNSS本质不同步。

图10

陀螺和加计标定误差估计结果不大，实际应用中如果惯导标定较好这些参数可以不列入状态，可降低Kalman滤波维数为19维。只需在KFApp.cpp文件“CKFApp::CKFApp(double ts):CSINSTDKF(34, 6)”语句中34改为19即可，非常简单，但注意输出文件“\data\kf.bin”的列数变化。