kalman.c

卡尔曼滤波源码

/* Copyright (c) 1994-98 by The MathWorks, Inc. */
/* $Revision: 1.6 $  $Date: 1997/12/01 21:45:35 $  $Author: moler $ */

/* get the data to be used in kalman filter algorithm */
/* off-line only */
static void
#ifdef __STDC__
anfisGetKalmanDataPair(FIS *fis, int which_data)
#else
anfisGetKalmanDataPair(fis, which_data)
FIS *fis;
int which_data;
#endif
{
	/* ========== */
	int linear_para_n = ((fis->order)*(fis->in_n)+1)*fis->rule_n;

	/* 'first' is the index of the first node in layer 3 */
	int first = fis->layer[3]->index;
	double total_firing_strength = 0;
	int i, j, k;

	for (i = first; i < first + fis->rule_n; i++)
		total_firing_strength += fis->node[i]->value;

	j = 0;
	for (i = first; i < first + fis->rule_n; i++) {
		/* ========== */
		if (fis->order == 1) {
			for (k = 0; k < fis->in_n; k++) 
				fis->kalman_io_pair[j++] =
					(fis->node[i]->value)*
					(fis->node[k]->value);
			fis->kalman_io_pair[j++] = fis->node[i]->value;
		} else {
			fis->kalman_io_pair[j++] = fis->node[i]->value;
		}
	}

	for (i = 0; i < linear_para_n; i++)
		fis->kalman_io_pair[i] /= total_firing_strength;

	for (i = 0; i < fis->out_n; i++)
		fis->kalman_io_pair[linear_para_n+i] =
			fis->trn_data[which_data][fis->in_n+i];
}


/* get the data to be used in ML algorithm */
/* off-line only */
static void
#ifdef __STDC__
anfisGetKalmanDataPair2(FIS *fis, int which_data)
#else
anfisGetKalmanDataPair2(fis, which_data)
FIS *fis;
int which_data;
#endif
{
	/* ========== */

	/* 'first' is the index of the first node in layer 3 */
	int first = fis->layer[1]->index;
	int i, j, k;

	j = 0;
        for (k=fis->in_n; k<fis->node_n-1; k++)
          for (i = 0; i < fis->node[k]->para_n; i++)
		fis->kalman_io_pair[j++] = fis->node[k]->de_dp[i];

	for (i = 0; i < fis->out_n; i++)
	  fis->kalman_io_pair[fis->para_n+i] =
	   (fis->trn_data[which_data][fis->in_n+i]-fis->node[fis->node_n-1]->value);
}


/* get the data to be used in kalman filter algorithm */
/* on-line only */
static void
#ifdef __STDC__
anfisGetKalmanDataPair1(FIS *fis, double *u)
#else
anfisGetKalmanDataPair1(fis, u)
FIS *fis;
double *u;
#endif
{
	int linear_para_n = (fis->in_n+1)*fis->rule_n;
	/* 'first' is the index of the first node in layer 3 */
	int first = fis->layer[3]->index;
	double total_firing_strength = 0;
	int i, j, k;

	for (i = first; i < first + fis->rule_n; i++)
		total_firing_strength += fis->node[i]->value;
	/*
	PRINT(total_firing_strength);
	*/

	j = 0;
	for (i = first; i < first + fis->rule_n; i++) {
		for (k = 0; k < fis->in_n; k++) 
			fis->kalman_io_pair[j++] = (fis->node[i]->value)*
				(fis->node[k]->value);
		fis->kalman_io_pair[j++] = fis->node[i]->value;
	}

	for (i = 0; i < linear_para_n; i++)
		fis->kalman_io_pair[i] /= total_firing_strength;

	for (i = 0; i < fis->out_n; i++)
		fis->kalman_io_pair[linear_para_n+i] = u[fis->in_n+i];

	/*
	PRINT(linear_para_n);
	PRINTARRAY(u, fis->in_n + fis->out_n);
	PRINTARRAY(fis->kalman_io_pair, linear_para_n+1);
	*/
}

/* put the parameters identified by kalman filter algorithm back into ANFIS */
static void
#ifdef __STDC__
anfisPutKalmanParameter(FIS *fis)
#else
anfisPutKalmanParameter(fis)
FIS *fis;
#endif
{
	/* 'first' is the index of the first node in layer 4 */
	int first = fis->layer[4]->index;
	int i, j, k;

	k = 0;
	for (i = first; i < first + fis->rule_n; i++) 
		for (j = 0; j < fis->node[i]->para_n; j++)
			fis->node[i]->para[j] = fis->kalman_para[k++][0];
}

/* put the parameters identified by LM algorithm back into ANFIS */
static void
#ifdef __STDC__
anfisPutKalmanParameter2(FIS *fis)
#else
anfisPutKalmanParameter2(fis)
FIS *fis;
#endif
{
	/* 'first' is the index of the first node in layer 4 */
	int first = fis->layer[1]->index;
	int i, j, k;

	k = 0;
        for (i=fis->in_n; i<fis->node_n-1; i++)
	  for (j = 0; j < fis->node[i]->para_n; j++)
	      fis->node[i]->para[j] += fis->kalman_para[k++][0];
}


/* matrix plus matrix */
static void
#ifdef __STDC__
anfisMplusM(double **m1, double **m2, int row, int col, double **out)
#else
anfisMplusM(m1, m2, row, col, out)
double **m1, **m2, **out;
int row, col;
#endif
{
	int i, j;
	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			out[i][j] = m1[i][j] + m2[i][j];
}

/* matrix minus matrix */
static void
#ifdef __STDC__
anfisMminusM(double **m1, double **m2, int row, int col, double **out)
#else
anfisMminusM(m1, m2, row, col, out)
double **m1, **m2, **out;
int row, col;
#endif
{
	int i, j;
	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			out[i][j] = m1[i][j] - m2[i][j];
}

/* matrix times matrix */
static void
#ifdef __STDC__
anfisMtimesM(double **m1, double **m2, int row1, int col1, int col2, double **out)
#else
anfisMtimesM(m1, m2, row1, col1, col2, out)
double **m1, **m2, **out;
int row1, col1, col2;
#endif
{
	int i, j, k;
	for (i = 0; i < row1; i++)
		for (j = 0; j < col2; j++) {
			out[i][j] = 0;
			for (k = 0; k < col1; k++)
				out[i][j] += m1[i][k]* m2[k][j];
		}
}

/* scalar times matrix */
static void
#ifdef __STDC__
anfisStimesM(double c, double **m, int row, int col, double **out)
#else
anfisStimesM(c, m, row, col, out)
double c;
double **m, **out;
int row, col;
#endif
{
	int i, j;
	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			out[i][j] = c*m[i][j];
}

/* matrix transpose */
static void
#ifdef __STDC__
transposeM(double **m, int row, int col, double **m_t)
#else
transposeM(m, row, col, m_t)
double **m, **m_t;
int row, col;
#endif
{
	int i, j;
	for (i = 0; i < row; i++)
		for (j = 0; j < col; j++)
			m_t[j][i] = m[i][j];
}

/* matrix L-2 norm */
double
#ifdef __STDC__
matrixNorm(double **m, int row, int col)
#else
matrixNorm(m, row, col)
double **m;
int row, col;
#endif
{
	int i, j;
	double total = 0;

	for (i = 0; i < col; i++)
		for (j = 0; j < row; j++)
			total += m[i][j]*m[i][j];
	return(sqrt(total));
}

/* same as kalman(), but with forgetting factor lambda */
/* reset != 0 --> reset matrices S and P */
static void
#ifdef __STDC__
anfisKalman(FIS *fis, int reset, double alpha)
#else
anfisKalman(fis, reset)
FIS *fis;
int reset;
double alpha;
#endif
{
	double **S, **P, **a, **b, **a_t, **b_t;
	double **tmp1, **tmp2, **tmp3, **tmp4;
	double **tmp5, **tmp6, **tmp7;

	/* ========== */
	int in_n;
	int out_n = fis->out_n;

	int i, j;
	double denom;
        
        if (fis->method==1)
         in_n = ((fis->order)*(fis->in_n)+1)*fis->rule_n;
        else if (fis->method==2)
         in_n = fis->para_n;
 
	S = fis->S;
	P = fis->P;
	a = fis->a;
	b = fis->b;
	a_t = fis->a_t;
	b_t = fis->b_t;
	tmp1 = fis->tmp1;
	tmp2 = fis->tmp2;
	tmp3 = fis->tmp3;
	tmp4 = fis->tmp4;
	tmp5 = fis->tmp5;
	tmp6 = fis->tmp6;
	tmp7 = fis->tmp7;

	/* reset S[][] and P[][] */
	if (reset != 0) {
/*======  already defined in param passed in
		double alpha;

                if (fis->method==1)
                     alpha = 1e6;
                else
                     alpha = 1e-1;           =======*/
		for (i = 0; i < in_n; i++)
			for (j = 0; j < out_n; j++)
				P[i][j] = 0;
		for (i = 0; i < in_n; i++)
			for (j = 0; j < in_n; j++)
				if (i == j)
					S[i][j] = alpha; 
				else
					S[i][j] = 0;
		return;
	}

	/* reset == 0, normal operation */
	for (i = 0; i < in_n; i++)
		a[i][0] = fis->kalman_io_pair[i];
	for (i = 0; i < out_n; i++)
		b[i][0] = fis->kalman_io_pair[i+in_n];

	transposeM(a, in_n, 1, a_t);
	transposeM(b, out_n, 1, b_t);

	/* recursive formulas for S, covariance matrix */
	anfisMtimesM(S, a, in_n, in_n, 1, tmp1);
	anfisMtimesM(a_t, tmp1, 1, in_n, 1, tmp2);
	denom = fis->lambda + tmp2[0][0];
	anfisMtimesM(a_t, S, 1, in_n, in_n, tmp3);
	anfisMtimesM(tmp1, tmp3, in_n, 1, in_n, tmp4);
	anfisStimesM(1/denom, tmp4, in_n, in_n, tmp4);
	anfisMminusM(S, tmp4, in_n, in_n, S);
	anfisStimesM(1/fis->lambda, S, in_n, in_n, S);

	/* recursive formulas for P, the estimated parameter matrix */
	anfisMtimesM(a_t, P, 1, in_n, out_n, tmp5);
	anfisMminusM(b_t, tmp5, 1, out_n, tmp5);
	anfisMtimesM(a, tmp5, in_n, 1, out_n, tmp6);
	anfisMtimesM(S, tmp6, in_n, in_n, out_n, tmp7);
	anfisMplusM(P, tmp7, in_n, out_n, P);

	for (i = 0; i < in_n; i++)
		for (j = 0; j < out_n; j++)
			fis->kalman_para[i][j] = P[i][j];
}