第十七篇，五阶多项式

做轨迹规划用到了五阶多项式曲线拟合，但MATLAB自带的polyfit()不知道实现细节，simulink模型code generation后生成的代码量太大，以至于在常规的DSP+ARM的MCU上完全跑不起来，怀疑是MATLAB的常见毛病：无差别矩阵运算带来了超量算力需求。

故而转向探索C/C++代码嵌入simulink模型中的方法，以C/C++的高效来解决现实问题。

主要参考了这篇博客，非常感谢作者，接下来我将要po的代码就算在您的基础上改的：

C实现的多项式拟合函数_TensorME的博客-CSDN博客_c多项式拟合

不关注代码的内部具体逻辑，不掌握背后如最小二乘等深层次数学原理的咱也看不懂，做黑盒测试验证可用性即可，再把集成的代码烧到板子上验证效率就算全部走通。

验证完发现是可用的，对标MATLAB的polyfit()也完全没有问题，效率更是杠杠的！

发现的不完美的地方以及一些说明如下：

• 原版M、N的注释写的有问题，按理说M=N-1，但传入M=N才能出来正确结果；
• CDoubleArray等数据类型的定义和使用不易理解，应该是还有别的代码文件作者没上传；
• 针对上条我做了适应性修改，改用double等常规的一目了然的定义；
• 拟合结果和MATLAB的polyfit()逆序，这不是bug，但为了对标以及方便调试，我在代码末尾做了逆序，和MATLAB的polyfit()保持一致。

po上重整后的.h.c，以飨读者：

#ifndef POLYFIT_YYP_h_
#define POLYFIT_YYP_h_

#include <stdlib.h>
/*#include <malloc.h>*/

/**
 * @brief 多项式拟合
 * @param X 自变量
 * @param Y 因变量
 * @param order 阶数
 * @param A 拟合结果，注意A的元素个数=order+1、高阶系数在前低阶系数在后跟MATLAB的polyfit()一致
 */
void CalculateCurveParameter(const double *X, const double *Y, const long order, double *A);

#endif

#include "PolyFit_YYP.h"

void CalculateCurveParameter(const double *X, const double *Y, const long order, double *A)
{
	long i,j,k;
	double Z,D1,D2,C,P,G,Q;

	long M, N;

	double* B = (double*)malloc(sizeof(double)*(order+1));
  double* T = (double*)malloc(sizeof(double)*(order+1));
  double* S = (double*)malloc(sizeof(double)*(order+1));
	
	M = order+1;
	N = order+1;

	if(M>N)M=N;
	for(i=0;i<M;i++)
		A[i]=0;
	Z=0;
	B[0]=1;
	D1=N;
	P=0;
	C=0;
	for(i=0;i<N;i++)
	{
		P=P+X[i]-Z;
		C=C+Y[i];
	}
	C=C/D1;
	P=P/D1;
	A[0]=C*B[0];
	if(M>1)
	{
		T[1]=1;
		T[0]=-P;
		D2=0;
		C=0;
		G=0;
		for(i=0;i<N;i++)
		{
			Q=X[i]-Z-P;
			D2=D2+Q*Q;
			C=Y[i]*Q+C;
			G=(X[i]-Z)*Q*Q+G;
		}
		C=C/D2;
		P=G/D2;
		Q=D2/D1;
		D1=D2;
		A[1]=C*T[1];
		A[0]=C*T[0]+A[0];
	}
	for(j=2;j<M;j++)
	{
		S[j]=T[j-1];
		S[j-1]=-P*T[j-1]+T[j-2];
		if(j>=3)
		{
			for(k=j-2;k>=1;k--)
				S[k]=-P*T[k]+T[k-1]-Q*B[k];
		}
		S[0]=-P*T[0]-Q*B[0];
		D2=0;
		C=0;
		G=0;
		for(i=0;i<N;i++)
		{
			Q=S[j];
			for(k=j-1;k>=0;k--)
				Q=Q*(X[i]-Z)+S[k];
			D2=D2+Q*Q;
			C=Y[i]*Q+C;
			G=(X[i]-Z)*Q*Q+G;
		}
		C=C/D2;
		P=G/D2;
		Q=D2/D1;
		D1=D2;
		A[j]=C*S[j];
		T[j]=S[j];
		for(k=j-1;k>=0;k--)
		{
			A[k]=C*S[k]+A[k];
			B[k]=T[k];
			T[k]=S[k];
		}
	}
	
  free(B);
  free(T);
  free(S);
  B = NULL;
  T = NULL;
  S = NULL;

/**
 * 逆序，和MATLAB的polyfit()保持一致
 */
  for (i=0; i<order/2+1; i++)
  {
	Z = A[i];
	A[i] = A[order-i];
	A[order-i] = Z;
  }
}