加权最小二乘法参数方程化拟合局部路径

最小二乘法拟合

基本原理：
https://baike.baidu.com/item/最小二乘法/2522346?fr=aladdin
基本公式：

，，

参数方程化：
由于在使用普通方程（这里使用五阶方程）求解矩阵时，要求X^TX可逆。但是我们在局部路径拟合时会经常遇到如下场景，在这种场景下无法求解：

所以将x和y参数方程化之后求解，就可以规避这个问题。
具体做法是：我们生成的局部路径是一串离散点，计算从起点到每个点经过的路径长度l_i以及总长度L。然后计算每个点对应的参数方程t的值：
t_i=l_i/L
再把t的值构造成参数矩阵，带入求解公式中即可求解出系数。
后来我使用加权最小二乘法来进行拟合，构造一个权重矩阵weight,使权重由近点到远点越来越小，远点的权重值为0，故远点几乎不对曲线造成影响，结果见下图。

clear
clc
close
x=[0,1,1.5,3,4,8,9,12,13,15];
y=[0,1,3,5,5,7,11,13,14,17];
% x=[0,0,0,0,0,0,0,0,0,0];
% y=[0,1,2,3,4,5,6,7,8,9];
[~,k]=size(x);% ~:忽略输出参数

xx=zeros(k,6);
l=zeros(1,k);
t=zeros(1,k);
weight=zeros(k,k);

for i=2:k%计算路径中每个点到起始点的路径长度
    l(i)=calcu_dis(x(i-1),y(i-1),x(i),y(i))+l(i-1);
end
for i=1:k
    t(i)=l(i)/l(k);%计算路径中每个点对应参数方程中t的值
    weight(i,i)=10000 - i*9900/k;%构造权重矩阵
end
for i=1:6%构造求解tt矩阵
    tt(:,i)=t'.^(6-i);
end
ansxt=(tt'*tt)\tt'*x';%求解xt参数方程对应的6个参数
ansyt=(tt'*tt)\tt'*y';
tt=weight*tt;
ansxt1=(tt'*tt)\tt'*weight*x';%求解加权拟合的xt参数方程对应的6个参数
ansyt1=(tt'*tt)\tt'*weight*y';

ft=0:0.01:1;
fxt=ansxt(1,1)*ft.^5 + ansxt(2,1)*ft.^4 + ansxt(3,1)*ft.^3 + ansxt(4,1)*ft.^2 + ansxt(5,1)*ft.^1 + ansxt(6,1);
fyt=ansyt(1,1)*ft.^5 + ansyt(2,1)*ft.^4 + ansyt(3,1)*ft.^3 + ansyt(4,1)*ft.^2 + ansyt(5,1)*ft.^1 + ansyt(6,1);
fxt1=ansxt1(1,1)*ft.^5 + ansxt1(2,1)*ft.^4 + ansxt1(3,1)*ft.^3 + ansxt1(4,1)*ft.^2 + ansxt1(5,1)*ft.^1 + ansxt1(6,1);
fyt1=ansyt1(1,1)*ft.^5 + ansyt1(2,1)*ft.^4 + ansyt1(3,1)*ft.^3 + ansyt1(4,1)*ft.^2 + ansyt1(5,1)*ft.^1 + ansyt1(6,1);
%画出求解结果

plot(fxt,fyt)
hold on

for i=1:6%构造求解xx矩阵
    xx(:,i)=x'.^(6-i);
end
ans=(xx'*xx)\xx'*y';%求解普通方程对应的6个参数

fx=0:0.01:17;
fy=ans(1,1)*fx.^5 + ans(2,1)*fx.^4 + ans(3,1)*fx.^3 + ans(4,1)*fx.^2 + ans(5,1)*fx.^1 + ans(6,1);
%画出求解结果

plot(x,y,'o')
hold on
plot(fx,fy)
hold on
title('普通方程拟合与参数方程拟合结果')

legend('参数方程拟合','原始数据点','普通方程拟合');

figure('Name','加权拟合与非加权拟合结果对比','NumberTitle','off')
suptitle('加权拟合与非加权拟合结果对比')
subplot(1,2,1)
plot(x,y,'o')
hold on
plot(fxt1,fyt1)
hold on
legend('原始数据点','加权拟合结果');

subplot(1,2,2)
plot(x,y,'o')
hold on
plot(fxt,fyt)
hold on
legend('原始数据点','非加权拟合结果');


function [ dis ] = calcu_dis( x1,y1,x2,y2 )
%CALCU_KPI 计算两个点之间的距离
dis=(x2-x1).^2+(y1-y2).^2;
dis=sqrt(dis);
end