離散點曲率擬合

有噪聲的離散點，計算曲率。首先用濾波法降噪，然後用三點法計算半徑

clc
clear all

load('D:\ccs\DaischTest\TraceC10.mat')

 spacelimit = 1;    
 start =2;
 finish =1;
x= PosLat(start : spacelimit:length(PosLat)-finish);
y = PosLon(start: spacelimit:length(PosLat)-finish);
ha = AngleHeading(start: spacelimit:length(PosLat)-finish);
time = Time(start: spacelimit:length(PosLat)-finish);

% sw = SAS_SteerWheelAngle(start: spacelimit:length(PosLat)-finish);

x = roundn(x, -8);
y =  roundn(y,-8);
B0 = x(1);
L0 = y(1);
HA= roundn(ha, -2);
% SW = vpa(sw,3);
% plot(x,y, 'LineWidth',10)

x%%

for  i = 1: length(x)
     [X(i),Y(i)]=transformation1(x(i),y(i),B0,L0);
     i
end

X = X';
Y = Y';

%%
figure 
for i = 1: 1:length(X)
% plot(X(3:i),Y(3:i),'-o')
% i
plot(X(i), Y(i), 'o')
hold on

%  drawnow
end

%% 計算公式
% x1 = 0; y1 = 0; x2 = 1;y2 =0; x3 = 1; y3 = 1;
% a=x1*(y2-y3)+x2*(y3-y1)+x3*(y1-y2)
% b=(x1^2+y1^2-x2^2-y2^2)/2
% c=(x1^2+y1^2-x3^2-y3^2)/2
% d=b*(y1-y3)-c*(y1-y2)
% e=c*(x1-x2)-b*(x1-x3)
% r=sqrt((d/a-x1)^2+(e/a-y1)^2)
figure
for i = 1:length(X)-5
    X(i) = 0.2*sum(X(i:i+4));
    Y(i) = 0.2 * sum(Y(i:i+4));
end
plot(X,Y,'b-')

figure
for i = 1: length(X)-5
    x1 = X(i); y1 = Y(i);
    x2 = X(i+2); y2 = Y(i+2);
    x3 = X(i+4); y3 = Y(i+4);
    a=x1*(y2-y3)+x2*(y3-y1)+x3*(y1-y2);
b=(x1^2+y1^2-x2^2-y2^2)/2;
c=(x1^2+y1^2-x3^2-y3^2)/2;
d=b*(y1-y3)-c*(y1-y2);
e=c*(x1-x2)-b*(x1-x3);
r(i)=sqrt((d/a-x1)^2+(e/a-y1)^2)
 if 1/r(i)>0.5
        r(i) = 0;
    end
end

plot(1./r,'r.')