sffisold.c

模糊逻辑工具箱 模糊逻辑工具箱 模糊逻辑工具箱 模糊逻辑工具箱 模糊逻辑工具箱

/*
 * SIMULINK MEX S-function for fuzzy controllers
 * J.-S. Roger Jang, 1994.
 * Copyright (c) 1994-98 by The MathWorks, Inc.
 * $Revision: $  $Date: $  
 */

/*
 * Syntax  [sys, x0] = sffis(t, x, u, flag, FISMATRIX)
 */

/*
 * The following #define is used to specify the name of this S-Function.
 */
#define S_FUNCTION_NAME sffis

#include <stdio.h>	/* needed for declaration of sprintf */
#include <stdlib.h>	/* needed for declaration of calloc */

/*
 * need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */
#include "simstruc.h"
/* For RTW */
#if defined(RT) || defined(NRT)  

#define __MEX_H__ /* don't include mex.h */

#undef  mexCallMATLAB
#define mexCallMATLAB(a,b,c,d,e) \
    fisError("User-defined functions for FLT are not allowed in RTW!\n");

#undef  mexPrintf
#define mexPrintf printf

#endif
#ifndef __FIS__
#define __FIS__

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

/***********************************************************************
 Macros and definitions
 **********************************************************************/

#ifndef ABS
#define ABS(x)   ( (x) > (0) ? (x): (-(x)) )
#endif
#ifndef MAX
#define MAX(x,y) ( (x) > (y) ? (x) : (y) )
#endif
#ifndef MIN
#define MIN(x,y) ( (x) < (y) ? (x) : (y) )
#endif
#define MF_PARA_N 4
#define STR_LEN 500
#define MF_POINT_N 101

/* debugging macros */
/*
#define PRINT(expr) printf(#expr " = %g\n", (double)expr)
#define PRINTMAT(mat,m,n) printf(#mat " = \n"); fisPrintMatrix(mat,m,n)
#define FREEMAT(mat,m) printf("Free " #mat " ...\n"); fisFreeMatrix(mat,m)
#define FREEARRAY(array) printf("Free " #array " ...\n"); free(array)
*/
#define FREEMAT(mat,m) fisFreeMatrix(mat,m)
#define FREEARRAY(array) free(array)

/***********************************************************************
 Data types
 **********************************************************************/

typedef struct fis_node {
	int handle;
	int load_param;
	char name[STR_LEN];
	char type[STR_LEN];
	char andMethod[STR_LEN];
	char orMethod[STR_LEN];
	char impMethod[STR_LEN];
	char aggMethod[STR_LEN];
	char defuzzMethod[STR_LEN];
	int userDefinedAnd;
	int userDefinedOr;
	int userDefinedImp;
	int userDefinedAgg;
	int userDefinedDefuzz;
	int in_n;
	int out_n;
	int rule_n;
	int **rule_list;
	double *rule_weight;
	int *and_or;	/* AND-OR indicator */
	double *firing_strength;
	double *rule_output;
	/* Sugeno: output for each rules */
	/* Mamdani: constrained output MF values of rules */
	struct io_node **input;
	struct io_node **output;
#ifdef __STDC__
	double (*andFcn)(double, double);
	double (*orFcn)(double, double);
	double (*impFcn)(double, double);
	double (*aggFcn)(double, double);
#else
	double (*andFcn)();
	double (*orFcn)();
	double (*impFcn)();
	double (*aggFcn)();
#endif
	double (*defuzzFcn)();
	double *BigOutMfMatrix;	/* used for Mamdani system only */
	double *BigWeightMatrix;/* used for Mamdani system only */
	double *mfs_of_rule;	/* MF values in a rule */
	struct fis_node *next;
} FIS;

typedef struct io_node {
	char name[STR_LEN];
	int mf_n;
	double bound[2];
	double value;
	struct mf_node **mf;
} IO;

typedef struct mf_node {
	char label[STR_LEN];
	char type[STR_LEN];
#ifdef __STDC__
	double (*mfFcn)(double, double *); /* pointer to a mem. fcn */ 
#else
	double (*mfFcn)(); /* pointer to a mem. fcn */ 
#endif
	double para[MF_PARA_N];
	double *sugeno_coef;	/* for Sugeno only */
	double value;		/* for Sugeno only */
	double *value_array;	/* for Mamdani only, array of MF values */
	int userDefined;	/* 1 if the MF is user-defined */
} MF;

#endif /* __FIS__ */
/***********************************************************************
 File, arrays, matrices operations 
 **********************************************************************/
/* Copyright (c) 1994-98 by The MathWorks, Inc. */
/* $Revision: $  $Date: $  */

/* display error message and exit */
static void
#ifdef __STDC__
fisError(char *msg)
#else
fisError(msg)
char *msg;
#endif
{
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt(msg);
#else
#ifndef NO_PRINTF
	printf(msg);
	printf("\n");
#endif
exit(1);
#endif
}

#ifndef NO_PRINTF         /*in case for rtw and dSPACE use */

/* an friendly interface to fopen() */
static FILE *
#ifdef __STDC__
fisOpenFile(char *file, char *mode)
#else
fisOpenFile(file, mode)
char *file;
char *mode;
#endif
{
	FILE *fp, *fopen();

	if ((fp = fopen(file, mode)) == NULL){
		printf("The file '%s'", file);
		fisError(" cannot be opened.\n");
	}
	return(fp);
}

#endif


char **
#ifdef __STDC__
fisCreateMatrix(int row_n, int col_n, int element_size)
#else
fisCreateMatrix(row_n, col_n, element_size)
int row_n;
int col_n;
int element_size;
#endif
{
	char **matrix;
	int i;

	if (row_n == 0 && col_n == 0)
		return(NULL);

	matrix = (char **)calloc(row_n, sizeof(char *));
	if (matrix == NULL)
		fisError("calloc error in fisCreateMatrix!");
	for (i = 0; i < row_n; i++) { 
		matrix[i] = (char *)calloc(col_n, element_size);
		if (matrix[i] == NULL)
			fisError("calloc error in fisCreateMatrix!");
	}
	return(matrix);
}

/* won't complain if given matrix is already freed */
static void
#ifdef __STDC__
fisFreeMatrix(void **matrix, int row_n)
#else
fisFreeMatrix(matrix, row_n)
void **matrix;
int row_n;
#endif
{
	int i;
	if (matrix != NULL) {
		for (i = 0; i < row_n; i++) {
			/*
			printf("i = %d\n", i);
			*/
			free(matrix[i]);
			/*
			matrix[i] = NULL;
			*/
		}
		free(matrix);
	}
}

/* complain if given matrix is already freed */
/*
static void
#ifdef __STDC__
fisFreeMatrix(void **matrix, int row_n)
#else
fisFreeMatrix(matrix, row_n)
void **matrix;
int row_n;
#endif
{
        int i;
        for (i = 0; i < row_n; i++) 
                if (matrix[i] == NULL)
#ifndef NO_PRINTF
                        printf("fisFreeMatrix: row %d is already free!\n", i);
#endif

                else {
                        free(matrix[i]);
                        matrix[i] = NULL;
                }

        if (matrix == NULL)
#ifndef NO_PRINTF
                printf("fisFreeMatrix: given matrix is already free!\n");
#endif
        else {
                free(matrix);
                matrix = NULL;
        }
}
*/

static double**
#ifdef __STDC__
fisCopyMatrix(double **source, int row_n, int col_n)
#else
fisCopyMatrix(source, row_n, col_n)
double **source;
int row_n;
int col_n;
#endif
{
	double **target;
	int i, j;

	target = (double **)fisCreateMatrix(row_n, col_n, sizeof(double));
	for (i = 0; i < row_n; i++)
		for (j = 0; j < col_n; j++)
			target[i][j] = source[i][j];
	return(target);
}

#ifndef NO_PRINTF        /* for rtw and dSPACE */
static void 
#ifdef __STDC__
fisPrintMatrix(double **matrix, int row_n, int col_n)
#else
fisPrintMatrix(matrix, row_n, col_n)
double **matrix;
int row_n;
int col_n;
#endif
{
	int i, j;
	for (i = 0; i < row_n; i++) {
		for (j = 0; j < col_n; j++)
			printf("%.3f ", matrix[i][j]);
		printf("\n");
	}
}

#endif

#ifndef NO_PRINTF   /* for dSPACE use */
static void
#ifdef __STDC__
fisPrintArray(double *array, int size)
#else
fisPrintArray(array, size)
double *array;
int size;
#endif
{
	int i;
	for (i = 0; i < size; i++)
		printf("%.3f ", array[i]);
	printf("\n");
}

#endif

#ifndef NO_PRINTF
static void
fisPause()
{
	printf("Hit RETURN to continue ...\n");
	getc(stdin);
}

#endif
/***********************************************************************
 Parameterized membership functions
 **********************************************************************/
/* Copyright (c) 1994-98 by The MathWorks, Inc. */
/* $Revision: $  $Date: $  */

/* Triangular membership function */
static double
#ifdef __STDC__
fisTriangleMf(double x, double *para)
#else
fisTriangleMf(x, para)
double x, *para;
#endif
{
	double a = para[0], b = para[1], c = para[2];

	if (a>b)
		fisError("Illegal parameters in fisTriangleMf() --> a > b");
	if (b>c)
		fisError("Illegal parameters in fisTriangleMf() --> b > c");

	if (a == b && b == c)
		return(x == a);
	if (a == b)
		return((c-x)/(c-b)*(b<=x)*(x<=c));
	if (b == c)
		return((x-a)/(b-a)*(a<=x)*(x<=b));
	return(MAX(MIN((x-a)/(b-a), (c-x)/(c-b)), 0));
}

/* Trapezpoidal membership function */
static double
#ifdef __STDC__
fisTrapezoidMf(double x, double *para)
#else
fisTrapezoidMf(x, para)
double x, *para;
#endif
{
	double a = para[0], b = para[1], c = para[2], d = para[3];
	double y1 = 0, y2 = 0;

	if (a>b) {
#ifndef NO_PRINTF
		printf("a = %f, b = %f, c = %f, d = %f\n", a, b, c, d);
#endif
		fisError("Illegal parameters in fisTrapezoidMf() --> a >= b");
	}
	if (c>d) {
#ifndef NO_PRINTF
		printf("a = %f, b = %f, c = %f, d = %f\n", a, b, c, d);
#endif
		fisError("Illegal parameters in fisTrapezoidMf() --> c >= d");
	}

	if (b <= x)
		y1 = 1;
	else if (x < a)
		y1 = 0;
	else if (a != b)
		y1 = (x-a)/(b-a);

	if (x <= c)
		y2 = 1;
	else if (d < x)
		y2 = 0;
	else if (c != d)
		y2 = (d-x)/(d-c);

	return(MIN(y1, y2));
	/*
	if (a == b && c == d)
		return((b<=x)*(x<=c));
	if (a == b)
		return(MIN(1, (d-x)/(d-c))*(b<=x)*(x<=d));
	if (c == d)
		return(MIN((x-a)/(b-a), 1)*(a<=x)*(x<=c));
	return(MAX(MIN(MIN((x-a)/(b-a), 1), (d-x)/(d-c)), 0));
	*/
}

/* Gaussian membership function */
static double
#ifdef __STDC__
fisGaussianMf(double x, double *para)
#else
fisGaussianMf(x, para)
double x, *para;
#endif
{
	double sigma = para[0], c = para[1];
	double tmp;

	if (sigma==0)
		fisError("Illegal parameters in fisGaussianMF() --> sigma = 0");
	tmp = (x-c)/sigma;
	return(exp(-tmp*tmp/2));
}

/* Extended Gaussian membership function */
static double
#ifdef __STDC__
fisGaussian2Mf(double x, double *para)
#else
fisGaussian2Mf(x, para)
double x, *para;
#endif
{
	double sigma1 = para[0], c1 = para[1];
	double sigma2 = para[2], c2 = para[3];
	double tmp1, tmp2;

	if ((sigma1 == 0) || (sigma2 == 0))
		fisError("Illegal parameters in fisGaussian2MF() --> sigma1 or sigma2 is zero");

	tmp1 = x >= c1? 1:exp(-pow((x-c1)/sigma1, 2.0)/2);
	tmp2 = x <= c2? 1:exp(-pow((x-c2)/sigma2, 2.0)/2);
	return(tmp1*tmp2);
}

/* Sigmoidal membership function */
static double
#ifdef __STDC__
fisSigmoidMf(double x, double *para)
#else
fisSigmoidMf(x, para)
double x, *para;
#endif
{
	double a = para[0], c = para[1];
	return(1/(1+exp(-a*(x-c))));
}

/* Product of two sigmoidal functions */
static double
#ifdef __STDC__
fisProductSigmoidMf(double x, double *para)
#else
fisProductSigmoidMf(x, para)
double x, *para;
#endif
{
	double a1 = para[0], c1 = para[1], a2 = para[2], c2 = para[3];
	double tmp1 = 1/(1+exp(-a1*(x-c1)));
	double tmp2 = 1/(1+exp(-a2*(x-c2)));
	return(tmp1*tmp2);
}

/* Absolute difference of two sigmoidal functions */
static double
#ifdef __STDC__
fisDifferenceSigmoidMf(double x, double *para)
#else
fisDifferenceSigmoidMf(x, para)
double x, *para;
#endif
{
	double a1 = para[0], c1 = para[1], a2 = para[2], c2 = para[3];
	double tmp1 = 1/(1+exp(-a1*(x-c1)));
	double tmp2 = 1/(1+exp(-a2*(x-c2)));
	return(fabs(tmp1-tmp2));
}

/* Generalized bell membership function */
static double
#ifdef __STDC__
fisGeneralizedBellMf(double x, double *para)
#else
fisGeneralizedBellMf(x, para)
double x, *para;
#endif
{
	double a = para[0], b = para[1], c = para[2];