多种频率之间导航系统的精度比较方法

在很多时候，为了验证自己开发的组合导航系统的精度，需要将组合导航系统和更高精度或绝对精确的设备进行比较，笔者在比较过程中遇到如下问题：

1.AUV在水下航行时受水流层扰动会导致路径非设定航线，而是围绕航线左右游摆，这样一条曲线因为阶次不定很难进行拟合

2.两种传感器采集频率不同，不能直接遍历求解误差，此外其中导航系统A由于水声通信过程中出现丢包等问题，导致数据出现较多时间内的丢失，而不是简单的采集频率不同。

本文对系统A使用插值法后再进行误差计算，具体步骤如下：

1.导入数据

clc
clear
close all
data=load('');
len=length(data);

data1=load('');
len1=length(data1);

a = 2;%插值阈值
x_auv=data(:,1);
y_auv=data(:,2);

x_750=data1(:,1);
y_750=data1(:,2);

其中，a为插值区间的判定阈值，只有当前后两值大于a时，才作为区间进行插值。

2.计算插值个数

num_insert = length(x_750) - length(x_auv);%需插值的坐标个数

将两种系统采集到的所有数据量之差作为插值个数

3.计算待插值区间

num_space = 0;%区间数
%x_insert待插区间坐标
j = 1;
k = 1;
m = 1;
for i=1:1:length(x_auv)-1 
    dist(i) = sqrt((x_auv(i+1)-x_auv(i))*(x_auv(i+1)-x_auv(i))+(y_auv(i+1)-y_auv(i))*(y_auv(i+1)-y_auv(i)));%1~len
    if(dist(i) >= a) 
        num_space = num_space+1;
        x_insert(j) = x_auv(i);
        x_insert_co(m) = i;
        m = m+1;
        x_insert(j+1) = x_auv(i+1);
        y_insert(j) = y_auv(i);
        y_insert(j+1) = y_auv(i+1);
        j = j+2;
        dist_insert(k) = dist(i);%各区间距离
        k = k+1;
    end  
end

通过遍历系统A数据，保存数据中插值区间的端点

4.分配插值量

dist_sum = sum(dist_insert);%坐标间总距离和
for i=1:1:num_space
    space_insert(i) = round(dist_insert(i)/dist_sum*num_insert); %各个区间应插入的坐标数  
end

做完除数的结果非整数，用round四舍五入

5.线性插值

w = 1; 
for i=1:1:num_space
    x=[x_insert(w),x_insert(w+1)];
    y=[y_insert(w),y_insert(w+1)];
    c = (x_insert(w+1)-x_insert(w)) / (space_insert(i)+1);
    xi = x_insert(w):c:x_insert(w+1);
    yi = interp1(x, y, xi, 'linear');
    w = w+2;
    space_insert1 = [0, space_insert];
    x_auv = [x_auv(1:x_insert_co(i)+sum(space_insert1(1:i))-1), xi, x_auv(x_insert_co(i)+sum(space_insert1(1:i))+2:length(x_auv))];
    y_auv = [y_auv(1:x_insert_co(i)+sum(space_insert1(1:i))-1), yi, y_auv(x_insert_co(i)+sum(space_insert1(1:i))+2:length(y_auv))];
end

interp1()函数需要自行设置x轴插点的间隔和首尾点，在插值后得到的是插值区间中的点，需将这些坐标插入到原数组。

6.计算误差

x_error=[];
y_error=[];
for i=1:1:length(x_750) 
    DIST = sqrt((x_auv(i)-x_750(i))*(x_auv(i)-x_750(i))+(y_auv(i)-y_750(i))*(y_auv(i)-y_750(i)));
    DIST_1 = sqrt((x_auv(i)-x_750(i)-x_error(1))*(x_auv(i)-x_750(i)-x_error(1))+(y_auv(i)-y_750(i)-y_error(1))*(y_auv(i)-y_750(i)-y_error(1)));
end
MAX = max(DIST);
MIN = min(DIST);
MEAN = mean(DIST);