calcphase.c

在c8051f015单片机上实现最小二乘法曲线拟合算法

#include "CalcPhase.h"
#include <math.h>

#define PI 3.1415926532

#define SAMPLE_NUMS 128   //采样数


typedef struct //定义平差系数结构
{
   float S_aa;
   float S_ab;
   float S_bb;
   float S_ll;
   float S_al;
   float S_bl;
}AVG_EQU_COEFF;


//-------------------------------平差函数--------------------------------------------------------
//输入：dat[]--测量数据数组 *amp-->初始振幅    *phase==初始初相角
//输出 *amp=平差后的振幅,*phase=平差后的初相角 
static void AvgErr(float dat[],float *amp,float *phase)
{
   UINT8 i,j;
   float XDATA_MEM ai,bi,li,temp,dA,dF;
   AVG_EQU_COEFF XDATA_MEM coeff;

   j=0;
   do
   {
      coeff.S_aa=0;
      coeff.S_ab=0;
      coeff.S_bb=0;
      coeff.S_ll=0;
      coeff.S_al=0;
      coeff.S_bl=0;
      for(i=0;i<SAMPLE_NUMS;i++)
      {
          temp=*phase+(float)PI*i*13/64;
          ai=sin(temp);
          bi=(*amp)*cos(temp);
          li=(*amp)*ai-dat[i];

          coeff.S_aa+=ai*ai;
          coeff.S_ab+=ai*bi;
          coeff.S_bb+=bi*bi;
          coeff.S_ll+=li*li;                                           
          coeff.S_al+=ai*li;
          coeff.S_bl+=bi*li;

      }
      temp=coeff.S_aa*coeff.S_bb-coeff.S_ab*coeff.S_ab;
      dA=(coeff.S_ab*coeff.S_bl-coeff.S_bb*coeff.S_al)/temp;
      dF=(coeff.S_ab*coeff.S_al-coeff.S_aa*coeff.S_bl)/temp;
      (*amp)+=dA;
      (*phase)+=dF;
      j++;
   }while((fabs(dF)>0.00125)&&(j<4));


}
     
//--------------------------------------根据测量数据计算初相位角函数-----------------------------
//dat---测量数据数组,长度128,（13个周期,等间隔采样128个数据）
//返回细分值(单位:等分数)
INT16 GetPhaseFromMeasDat(INT16 dat[])
{
   UINT8 i;   
   float fi,A;
   float XDATA_MEM dat_avg[SAMPLE_NUMS];
   float base=0;

   for(i=0;i<SAMPLE_NUMS;i++)base+=dat[i]; //求基准电平
   base/=SAMPLE_NUMS;

   for(i=0;i<SAMPLE_NUMS;i++)dat_avg[i]=(float)dat[i]-base; //求正弦值

   fi=atan2(dat_avg[0],dat_avg[32]);     //解算初始振幅及初相角

   A=sqrt((float)dat_avg[0]*dat_avg[0]+dat_avg[32]*dat_avg[32]);

   AvgErr(dat_avg,&A,&fi);  //平差

   while(fi<0)   //转换到正值
   {
      fi+=2*PI;
   }
   return fi*1250/PI+0.5;  //转换为细分值

}