clc;
clear;

filename = {'1.txt','2.txt'};

for i = 1:2
    data = csvread(filename{i});
    % 分别提取 x、y、xf 和 yf 列
    x = data(:, 1);
    y = data(:, 2);
    
    len = length(x);
    % 速度分析
    v = zeros(1,len);
    for i = 2:len-1
        d2 = (x(i) - x(i-1))^2 + (y(i) - y(i-1))^2;
        if(d2 == 0)
            v(i) = v(i-1);
        else
            % 采样频率是10ms
            v(i) = sqrt(d2);
        end
    end
    figure;
    plot(v);

    figure;
    % 滤波效果
    plot(x,y,'b.');

    x0 = x(1);
    y0 = y(1);
    xf = zeros(1,len);
    yf = zeros(1,len);
    xf(1) = x0;
    yf(1) = y0;
    alpha = 0.98;
    for i = 2:len
        if(v(i) < 40)
            alpha = 0.995;
        else
            alpha = 0.999;
        end
        xf(i) = alpha*xf(i-1) + (1 - alpha)*x(i);
        yf(i) = alpha*yf(i-1) + (1 - alpha)*y(i);
    end
    hold on;
    plot(xf,yf,'r.');
end

