In many cases, in order to verify the accuracy of the integrated navigation system developed by ourselves, it is necessary to compare the integrated navigation system with the equipment with higher accuracy or absolute accuracy. In the process of comparison, the author encountered the following problems:

1. When AUV is under water, it will be disturbed by the flow layer, which will lead to the route not set, but swing around the route left and right. Such a curve is difficult to fit because of the uncertain order

2. The acquisition frequency of the two sensors is different, so it can not directly traverse the solution error. In addition, due to the packet loss and other problems in the process of underwater acoustic communication, the navigation system A results in the loss of data in more time than the simple acquisition frequency.

In this paper, the interpolation method is used to calculate the error of system A, and the specific steps are as follows:

1. Import data

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

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

a = 2;%Interpolation threshold
x_auv=data(:,1);
y_auv=data(:,2);

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

Among them, a is the decision threshold of interpolation interval, and only when the last two values are greater than a, it can be used as interval for interpolation.

2. Calculate the number of interpolation

Num < insert = length (x < 750) - length (x < AUV);% number of coordinates to be interpolated

Take the difference of all data collected by the two systems as the number of interpolation

3. Calculate the interval to be interpolated

num_space = 0;%Interval numbers
%x_insert Interval coordinates to be inserted
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);%Distance between sections
        k = k+1;
    end  
end

By traversing system A data, the endpoint of interpolation interval in the data is saved

4. Allocate interpolation

dist_sum = sum(dist_insert);%Total distance between coordinates and
for i=1:1:num_space
    space_insert(i) = round(dist_insert(i)/dist_sum*num_insert); %Number of coordinates to be inserted in each interval  
end

The result of divisor is not an integer, round

5. Linear interpolation

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

The interp1() function needs to set the interval and the first and last points of the x-axis interpolation points by itself. After interpolation, the points in the interpolation interval are obtained, and these coordinates need to be inserted into the original array.

6. Calculation error

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);