正交多项式曲线拟合(MATLAB代码)
2024-04-14 14:51:42
文章目录
- 一、 正交多项式曲线拟合
- 1.曲线不经过起点与终点
- 2.曲线经过起点与终点
- 二、参考文献
一、 正交多项式曲线拟合
1.曲线不经过起点与终点
%{Function: calculate_orthogonal_polynomial_params
Description: 利用递推关系计算正交多项式参数alpha，beta(曲线不经过起点与终点)
Input: 点序列(ti, qi)的横坐标序列t，多项式次数m
Output: 正交多项式参数alpha，beta
Author: Marc Pony(marc_pony@163.com)
%}
function [alpha, beta] = calculate_orthogonal_polynomial_params(t, m)
if m == 0alpha = [];beta = [];return;
endif m == 1p = ones(size(t));alpha = sum(t .* p.^2) / sum(p.^2);beta = [];return;
endalpha = zeros(m, 1);
beta = zeros(m - 1, 1);
p0 = ones(size(t));
alpha(1) = sum(t .* p0.^2) / sum(p0.^2);
p1 = (t - alpha(1)) .* p0;
for i = 2 : malpha(i) = sum(t .* p1.^2) / sum(p1.^2);beta(i - 1) = sum(p1.^2) / sum(p0.^2);p2 = (t - alpha(i)) .* p1 - beta(i - 1) * p0;p0 = p1;p1 = p2;
end
end
%{Function: evaluate_orthogonal_polynomial
Description: 利用递推关系计算正交多项式pm在t处自0阶到2阶导数值(曲线不经过起点与终点)
Input: 横坐标t，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial(t, m, alpha, beta)
p0 = 1.0;
dp0 = 0.0;
ddp0 = 0.0;
p = p0;
dp = dp0;
ddp = ddp0;
if m == 0return;
endp1 = (t - alpha(1)) * p0;
dp1 = p0 + (t - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (t - alpha(1)) * ddp0;
p = p1;
dp = dp1;
ddp = ddp1;
if m == 1return;
endfor i = 2 : mp2 = (t - alpha(i)) * p1 - beta(i - 1) * p0;dp2 = p1 + (t - alpha(i)) * dp1 - beta(i - 1) * dp0;ddp2 = 2.0 * dp1 + (t - alpha(i)) * ddp1 - beta(i - 1) * ddp0;p0 = p1;dp0 = dp1;ddp0 = ddp1;p1 = p2;dp1 = dp2;ddp1 = ddp2;
end
p = p2;
dp = dp2;
ddp = ddp2;
end
%{Function: evaluate_orthogonal_polynomial_and_derivatives
Description: 利用递推关系计算正交多项式p0~pm在t处自0阶到2阶导数值(曲线不经过起点与终点)
Input: 横坐标t，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式p0~pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(t, m, alpha, beta)
p = zeros(m + 1, 1);
dp = zeros(m + 1, 1);
ddp = zeros(m + 1, 1);p0 = 1.0;
dp0 = 0.0;
ddp0 = 0.0;
p(1) = p0;
dp(1) = dp0;
ddp(1) = ddp0;
if m == 0return;
endp1 = (t - alpha(1)) * p0;
dp1 = p0 + (t - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (t - alpha(1)) * ddp0;
p(2) = p1;
dp(2) = dp1;
ddp(2) = ddp1;
if m == 1return;
endfor i = 2 : mp2 = (t - alpha(i)) * p1 - beta(i - 1) * p0;dp2 = p1 + (t - alpha(i)) * dp1 - beta(i - 1) * dp0;ddp2 = 2.0 * dp1 + (t - alpha(i)) * ddp1 - beta(i - 1) * ddp0;p(i + 1) = p2;dp(i + 1) = dp2;ddp(i + 1) = ddp2;p0 = p1;dp0 = dp1;ddp0 = ddp1;p1 = p2;dp1 = dp2;ddp1 = ddp2;
end
end
clc;
clear;
close all;t = [0, 1, 3, 7, 8, 10]'; %单调递增时间序列
q = [2, 3, 5, 6, 8, 9]';
m = 4; %多项式阶数（非负整数)
dt = 0.001;
n = length(t);[alpha, beta] = calculate_orthogonal_polynomial_params(t, m);a = zeros(m + 1, 1);
for j = 0 : msumValue = zeros(2, 1);for k = 1 : n[pj, ~, ~] = evaluate_orthogonal_polynomial(t(k), j, alpha, beta);sumValue(1) = sumValue(1) + q(k) * pj;sumValue(2) = sumValue(2) + pj^2;endif abs(sumValue(2)) > 1.0e-20a(j + 1) = sumValue(1) / sumValue(2);elsea(j + 1) = 0.0end
endposArray = [];
velArray = [];
accArray = [];
tArray = (t(1) : dt : t(end))';
for i = 1 : length(tArray)tt = tArray(i);[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(tt, m, alpha, beta);pos = dot(a, p);vel = dot(a, dp);acc = dot(a, ddp);posArray = [posArray; pos];velArray = [velArray; vel];accArray = [accArray; acc];
endif abs(tArray(end) - t(end)) > 1.0e-8[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(t(end), m, alpha, beta);pos = dot(a, p);vel = dot(a, dp);acc = dot(a, ddp);tArray = [tArray; t(end)];posArray = [posArray; pos];velArray = [velArray; vel];accArray = [accArray; acc];
endfigure(1)
subplot(3, 1, 1)
plot(tArray, posArray);
hold on
plot(t, q, '+');
xlabel('t');
ylabel('Position');subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('Velocity');subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('Acceleration');
2.曲线经过起点与终点
%{Function: calculate_orthogonal_polynomial_params2
Description: 利用递推关系计算正交多项式参数alpha，beta(曲线经过起点与终点)
Input: 归一化后的点序列横坐标tau，多项式次数m
Output: 正交多项式参数alpha，beta
Author: Marc Pony(marc_pony@163.com)
%}
function [alpha, beta] = calculate_orthogonal_polynomial_params2(tau, m)
if m == 0alpha = [];beta = [];return;
endif m == 1p = tau .* (1.0 - tau);alpha = sum(tau .* p.^2) / sum(p.^2);beta = [];return;
endalpha = zeros(m, 1);
beta = zeros(m - 1, 1);
p0 = tau .* (1.0 - tau);
alpha(1) = sum(tau .* p0.^2) / sum(p0.^2);
p1 = (tau - alpha(1)) .* p0;
for i = 2 : malpha(i) = sum(tau .* p1.^2) / sum(p1.^2);beta(i - 1) = sum(p1.^2) / sum(p0.^2);p2 = (tau - alpha(i)) .* p1 - beta(i - 1) * p0;p0 = p1;p1 = p2;
end
end
%{Function: evaluate_orthogonal_polynomial2
Description: 利用递推关系计算正交多项式pm在t处自0阶到2阶导数值(曲线经过起点与终点)
Input: 归一化横坐标tau，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial2(tau, m, alpha, beta)
p0 = tau * (1.0 - tau);
dp0 = 1.0 - 2.0 * tau;
ddp0 = -2.0;
p = p0;
dp = dp0;
ddp = ddp0;
if m == 0return;
endp1 = (tau - alpha(1)) * p0;
dp1 = p0 + (tau - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (tau - alpha(1)) * ddp0;
p = p1;
dp = dp1;
ddp = ddp1;
if m == 1return;
endfor i = 2 : mp2 = (tau - alpha(i)) * p1 - beta(i - 1) * p0;dp2 = p1 + (tau - alpha(i)) * dp1 - beta(i - 1) * dp0;ddp2 = 2.0 * dp1 + (tau - alpha(i)) * ddp1 - beta(i - 1) * ddp0;p0 = p1;dp0 = dp1;ddp0 = ddp1;p1 = p2;dp1 = dp2;ddp1 = ddp2;
end
p = p2;
dp = dp2;
ddp = ddp2;
end
%{Function: evaluate_orthogonal_polynomial_and_derivatives2
Description: 利用递推关系计算正交多项式p0~pm在t处自0阶到2阶导数值(曲线经过起点与终点)
Input: 归一化横坐标tau，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式p0~pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(tau, m, alpha, beta)
p = zeros(m + 1, 1);
dp = zeros(m + 1, 1);
ddp = zeros(m + 1, 1);p0 = tau * (1.0 - tau);
dp0 = 1.0 - 2.0 * tau;
ddp0 = -2.0;
p(1) = p0;
dp(1) = dp0;
ddp(1) = ddp0;
if m == 0return;
endp1 = (tau - alpha(1)) * p0;
dp1 = p0 + (tau - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (tau - alpha(1)) * ddp0;
p(2) = p1;
dp(2) = dp1;
ddp(2) = ddp1;
if m == 1return;
endfor i = 2 : mp2 = (tau - alpha(i)) * p1 - beta(i - 1) * p0;dp2 = p1 + (tau - alpha(i)) * dp1 - beta(i - 1) * dp0;ddp2 = 2.0 * dp1 + (tau - alpha(i)) * ddp1 - beta(i - 1) * ddp0;p(i + 1) = p2;dp(i + 1) = dp2;ddp(i + 1) = ddp2;p0 = p1;dp0 = dp1;ddp0 = ddp1;p1 = p2;dp1 = dp2;ddp1 = ddp2;
end
end
clc;
clear;
close all;t = [0, 1, 3, 7, 8, 10]'; %单调递增时间序列
q = [2, 3, 5, 6, 8, 9]';
m = 4; %多项式阶数（非负整数)
dt = 0.001;
n = length(t);tau = (t - t(1)) / (t(n) - t(1));
q_ = q - q(1) * (1 - tau) - q(n) * tau;[alpha, beta] = calculate_orthogonal_polynomial_params2(tau, m);a = zeros(m + 1, 1);
for j = 0 : msumValue = zeros(2, 1);for k = 1 : n[pj, ~, ~] = evaluate_orthogonal_polynomial2(tau(k), j, alpha, beta);sumValue(1) = sumValue(1) + q_(k) * pj;sumValue(2) = sumValue(2) + pj^2;endif abs(sumValue(2)) > 1.0e-20a(j + 1) = sumValue(1) / sumValue(2);elsea(j + 1) = 0.0;end
endposArray = [];
velArray = [];
accArray = [];
tArray = (t(1) : dt : t(end))';
for i = 1 : length(tArray)tau = (tArray(i) - t(1)) / (t(n) - t(1));[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(tau, m, alpha, beta);pos = dot(a, p) + q(1) * (1.0 - tau^2) + q(n) * tau^2;vel = dot(a, dp) + 2.0 * tau * (q(n) - q(1));acc = dot(a, ddp) + 2.0 * (q(n) - q(1));posArray = [posArray; pos];velArray = [velArray; vel];accArray = [accArray; acc];
endif abs(tArray(end) - t(end)) > 1.0e-8[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(1.0, m, alpha, beta);pos = dot(a, p) + q(n);vel = dot(a, dp) + 2.0 * (q(n) - q(1));acc = dot(a, ddp) + 2.0 * (q(n) - q(1));tArray = [tArray; t(end)];posArray = [posArray; pos];velArray = [velArray; vel];accArray = [accArray; acc];
endfigure(1)
subplot(3, 1, 1)
plot(tArray, posArray);
hold on
plot(t, q, '+');
xlabel('t');
ylabel('Position');subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('Velocity');subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('Acceleration');
二、参考文献
Trajectory Planning for Automatic Machines and Robots中章节：4.2 Orthogonal Polynomials
正交多项式曲线拟合(MATLAB代码)相关推荐
- matlab如何做正交多项式曲线拟合,matlab正交多项式拟合
在实验模态分析中用 Matlab 实现离散化正交多项式算法 [C], 马永列; 陈章 位; 胡海清 4.在实验模态分析中用 Matlab 实现离散化正交多项式算法 [C], 马永列...... 变换后 ...
- 最小二乘法曲线拟合程序matlab,最小二乘法曲线拟合（代码环境：matlab）
题目一: 1.用表1-1中的世界人口统计数值估计1980年的人口,求最佳最小二乘法数值估计: 表 1-1: 年 人口 1960 3 039 585 530 1970 3 707 475 887 199 ...
- 11种图像清晰度评价函数附MATLAB代码
本科毕业论文"基于图像处理的自动对焦技术研究",对焦过程中的一个重要阶段是图像清晰度评价,我用MATLAB实现了4类清晰度评价函数:基于图像梯度的清晰度评价函数.频域评价函数.信息 ...
- 非均匀三次B样条曲线插值实现及MATLAB代码
这篇博客跟我上一篇博客<均匀三次B样条曲线插值实现及MATLAB代码>的内容有点像,只是在基函数的计算上不同,造成均匀/非均匀的区别. 参考资料: [1](这个PPT讲得很通俗,但对于多插 ...
- 均匀三次B样条曲线插值实现及MATLAB代码
参考资料: [1](这个PPT讲得很通俗,但对于多插值点分段曲线的内容漏讲了一个知识点)三次周期B样条曲线的算法 - 百度文库 (baidu.com) [2](这个介绍只有两个插值点的三次B样条曲线, ...
- 扩展卡尔曼滤波（EKF）估计SOC代码2详解，基于二阶RC模型（附MATLAB代码）
上次分享了一个扩展卡尔曼滤波估计SOC的代码,得到了很多小伙伴的支持,今天再分享一个很好用的扩展卡尔曼滤波估计SOC的程序.使用MATLAB语言完成程序的编写. 有关EKF的推导及原理请看我写的另一个 ...
- 龙格-库塔法(runge-kutta)matlab代码及含义,龙格-库塔法(Runge-Kutta)matlab代码及含义...
龙格-库塔法(Runge-Kutta)matlab代码及含义 龙格-库塔法(Runge-Kutta) 数值分析中,龙格-库塔法(Runge-Kutta)是用于模拟常微分方程的解的重要的一类隐式或显式迭 ...
- arima模型matlab代码_PSTR面板平滑转换模型简介（附Matlab代码分享）
写论文的时候用到的~相关的资料太少了,做一些简单内容和资料的分享.(PSTR模型的Matlab代码分享在最后)本文主要为简单理论和粗暴实操~ 有用的话可以点个赞哟(知乎小白卑微求赞) 嘻嘻下面进入正题 ...
- matlab数值分析拟合实例,数值分析函数拟合matlab代码.doc
数值分析函数拟合matlab代码.doc 第一题MATLAB代码用SPLINE作图XI0204060810YI098092081064038X10012Y1NEWTON3XI,YI,X源代码见M文件Y ...
最新文章
- FiM | 牧医所奶业创新团队建立瘤胃微生物脲酶的靶向宏蛋白质组方法
- Hbase表结构设计
- 什么是COM与DCOM
- ⚡关于Eastmount博客「网络安全自学篇」系列重要通知！！！⚡
- 计算机网络【六】网络层协议
- 怎么跟踪php代码,第九节 PHP 跟踪调试代码 XDebug
- 开源/免费数学书大合集：微积分、线代、数分、抽代…数学教授分类整理，精心推荐...
- Qt之音频播放(二)
- 如何基于链表实现 LRU 缓存淘汰算法？
- Visio使用遇到的问题
- paip.提高效率---集合的存取括号方式 uapi java python php js 的实现比较
- postman更换皮肤
- 中国科学院大学计算机非全日制,中国科学院大学能考非全日制研究生？
- 【人工智能】谓词表示法与产生式知识表示实验
- Redhat_rhel_linux镜像下载，持续更新......
- 剪辑视频调整视频播放倍速，改变视频时长
- 计算机是如何执行程序的（转）
- AttributeError: 'bytes' object has no attribute '__dict__'
- [SEO工具]新站优化推广工具集
- 对于LSM Tree写放大问题的一些浅薄学习