calc.cpp

综合利用三次法和blewitt法检测与修复周跳

#include "stdafx.h"

#ifndef _CALC_H_
#include "Calc.h"
#endif

#include <stdio.h>//NULL defined
#include <math.h>



int PolyFit( const double x[], const double y[], const long int N, int M,
			 double a[], double dt[], int tag )
{
	int RunCode = M;

	int i, j, k;
	double z, p, c, g, q, d1, d2, s[MAXM+1], t[MAXM+1], b[MAXM+1];

	//should be modified
	double temp[MAXM+1];

	if ( N < 2 || M < 1 )//M<0???
	{
		RunCode = POLYFIT_ERR_PARAMETER;
		goto FINISH;
	}
	if ( M > N-1 )
	{
		M = N-1;
		RunCode = M;
	}
	if ( M > MAXM )
	{
		M = MAXM;
		RunCode = M;
	}

	switch( tag )
	{
	case LEASTSQUARE:
		{
			for ( i = 0; i < M+1; i++ )
				a[i] = 0.0;

			z = 0.0;
			for ( i = 0; i < N; i++ )
				z += x[i] / ( double )N;
			b[0] = 1.0;
			d1 = ( double )N;
			p = 0.0;
			c = 0.0;
			for ( i = 0; i < N; i++ )
			{
				p += ( x[i] - z );		//避免溢出的措施
				c += y[i];
			}
			c /= d1;				//mean_y
			p /= d1;				//mean_x~//NOTE:p=0;
			a[0] = c * b[0];		//和一阶拟和结论相仿
			

			if ( M > 0 )
			{
				t[1] = 1.0;
				t[0] = -p;
				d2 = 0.0;
				c = 0.0;
				g = 0.0;
				for ( i = 0; i < N; i++ )
				{
					q = x[i] - z - p;
					d2 += q * q;
					c += y[i] * q;
					g += ( x[i] - z ) * q * q;
				}
				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+1; 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.0;
					c = 0.0;
					g = 0.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 += q * q;
						c += y[i] * q;
						g += ( x[i] - z ) * q * q;
					}
					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];
					}
			}

			//should be modified
			temp[0] = - a[M] * z + a[M-1];
			temp[1] = a[M];

			for ( i = 1; i < M; i++ )
			{
				temp[i+1] = temp[i];
				for ( j = i; j > 0; j-- )
				{
					temp[j] = - temp[j] * z + temp[j-1];
				}
				temp[0] = - temp[0] * z + a[M-i-1];
			}

			for ( i = 0; i <= M; i++ )
			{
				a[i] = temp[i];
			}



			if ( dt != NULL )
			{
				dt[0] = 0.0;
				dt[1] = 0.0;
				dt[2] = 0.0;
				for ( i = 0; i < N; i++ )
				{
					q = 0.0;
					for ( k = 0; k <= M; k++ )
					{
						q += a[k] * pow( x[i], k );
					}
						p = q - y[i];
						if ( fabs( p ) > dt[2] )
							dt[2] = fabs( p );
						dt[0] += p * p;
						dt[1] += fabs( p );
				}
				dt[0] = sqrt( dt[0] );
			}
				
		}
		break;
	case CHEBYSHEV:
		{
		}
		break;
	default:
		{
			RunCode = POLYFIT_ERR_TAG;
		}
		break;
	}

FINISH:
	return RunCode;
}


double PolyVal( double x, const double a[], const unsigned int M )
{
	int i;
	double value = 0.0;
	for ( i = 0; i <= ( int )M; i++ )
	{
		value += a[i] * pow( x, i );
	}
	return value;
}


double RoundOff( double x )
{
	double i, f;
	if ( fabs( f = modf( x, &i ) ) >= 0.5 )
	{
		i += i / fabs( i );
	}
	return i;
}