kaltest.cpp

基于C的Kalman滤波源码

// Kaltest.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include "stdlib.h"
#include <stdio.h>
#include <malloc.h>
#include <math.h>

int klman(int n, int m, int k, double f[], double q[], double r[], double h[], double y[], double x[], double p[], double g[]);
//n 是时间更新（动态）系统的维数
//m 是观测系统的维数
//k 是观测序列的长度
//f, n x n 阶增益矩阵，将k-1时刻状态与K时刻状态联系起来
//q, n x n 阶矩阵，是差分方程（模型）中w的过程激励噪声的协方差矩阵
//r, m x m 阶矩阵，是观测噪声的协方差矩阵
//h, m x n 阶矩阵，是观测矩阵
//y, k x M 阶矩阵，是观测向量序列y(i,j), (j=1,.....m)，表示第i时刻的观测向量
//x, k x n 阶矩阵，输入兼输出，
//p, n x n 阶矩阵，输入兼输出，调用时存放p0，返回时存放最后时刻的估计误差协方差矩阵
//g, n x m 阶矩阵，输出参数，存放最后时刻稳定增益矩阵

int rinv(int n, double a[]);

int main(int argc, char* argv[])
{
	
	

 




	//printf("Hello World!\n");
	return 0;
}


//
//-------------------------------------------------------------------------------------------------------------------
//离散线性 Kalman filter
int klman(int n, int m, int k, double f[], double q[], double r[], double h[], double y[], double x[], double p[], double g[])
{
	int i,j,kk,ii,l,jj,js;
	double *e,*a,*b;
	//extern int brinv();
	e=(double *)malloc(m*m*sizeof(double));
	l=m;
	if (l<n) l=n;
	a=(double *)malloc(l*l*sizeof(double));
	b=(double *)malloc(l*l*sizeof(double));
	
	for (i=0; i<=n-1; i++)
		for (j=0; j<=n-1; j++)
		{
			ii=i*l+j; a[ii]=0.0;
			for (kk=0; kk<=n-1; kk++)
				a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];
		}
		
	for (i=0; i<=n-1; i++)
		for (j=0; j<=n-1; j++)
		{
			ii=i*n+j; p[ii]=q[ii];
			for (kk=0; kk<=n-1; kk++)
				p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];
		}
			
		for (ii=2; ii<=k; ii++)
		{
			for (i=0; i<=n-1; i++)
				for (j=0; j<=m-1; j++)
				{
					jj=i*l+j; a[jj]=0.0;
					for (kk=0; kk<=n-1; kk++)
						a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];
				}
					
			for (i=0; i<=m-1; i++)
				for (j=0; j<=m-1; j++)
				{
					jj=i*m+j; e[jj]=r[jj];
					for (kk=0; kk<=n-1; kk++)
						e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];
				}
						
			js=rinv(m,e);
			if (js==0) 
			{
				free(e);
				free(a);
				free(b);
				return(js);
			}
			
			for (i=0; i<=n-1; i++)
				for (j=0; j<=m-1; j++)
				{
					jj=i*m+j; g[jj]=0.0;
					for (kk=0; kk<=m-1; kk++)
						g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];
				}
				
			for (i=0; i<=n-1; i++)
			{
				jj=(ii-1)*n+i; x[jj]=0.0;
				for (j=0; j<=n-1; j++)
					x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];
			}
			
			for (i=0; i<=m-1; i++)
			{
				jj=i*l; b[jj]=y[(ii-1)*m+i];
				for (j=0; j<=n-1; j++)
					b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
			}
			
			for (i=0; i<=n-1; i++)
			{
				jj=(ii-1)*n+i;
				for (j=0; j<=m-1; j++)
					x[jj]=x[jj]+g[i*m+j]*b[j*l];
			}
			
			if (ii<k)
			{
				for (i=0; i<=n-1; i++)
					for (j=0; j<=n-1; j++)
					{
						jj=i*l+j; a[jj]=0.0;
						for (kk=0; kk<=m-1; kk++)
							a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];
						if (i==j) a[jj]=1.0+a[jj];
					}
						
				for (i=0; i<=n-1; i++)
					for (j=0; j<=n-1; j++)
					{
						jj=i*l+j; b[jj]=0.0;
						for (kk=0; kk<=n-1; kk++)
							b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j];
					}

				for (i=0; i<=n-1; i++)
					for (j=0; j<=n-1; j++)
					{
						jj=i*l+j; a[jj]=0.0;
						for (kk=0; kk<=n-1; kk++)
							a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];
					}

				for (i=0; i<=n-1; i++)
					for (j=0; j<=n-1; j++)
					{
						jj=i*n+j; p[jj]=q[jj];
						for(kk=0; kk<=n-1; kk++)
							p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];
					}
				}
			}
			
			free(e);
			free(a);
			free(b);
			
			return(js);
}


//
//--------------------------------------------------------------------------------------------------------------------
//矩阵求逆
int rinv(int n, double a[])
{
	int *is,*js,i,j,k,l,u,v;
	double d,p;
	
	is=(int *)malloc(n*sizeof(int));
	js=(int *)malloc(n*sizeof(int));

	for (k=0; k<=n-1; k++)
	{
		d=0.0;
		for (i=k; i<=n-1; i++)
			for (j=k; j<=n-1; j++)
			{
				l=i*n+j; p=fabs(a[l]);
				if (p>d) { d=p; is[k]=i; js[k]=j;
				}
			}
		
		if (d+1.0==1.0)
		{
			free(is);
			free(js);
			printf("err**not inv\n");
			return(0);
		} 

	if (is[k]!=k)
		for (j=0; j<=n-1; j++)
		{
			u=k*n+j;
			v=is[k]*n+j;
			p=a[u]; a[u]=a[v]; a[v]=p;
		} 

	if (js[k]!=k)
		for (i=0; i<=n-1; i++)
		{
			u=i*n+k;
			v=i*n+js[k];
			p=a[u]; a[u]=a[v]; a[v]=p;
		} 

	l=k*n+k;
	
	
	a[l]=1.0/a[l];
	for (j=0; j<=n-1; j++)
		if (j!=k)
		{
			u=k*n+j;
			a[u]=a[u]*a[l];
		}

	for (i=0; i<=n-1; i++)
		if (i!=k)
			for (j=0; j<=n-1; j++)
				if (j!=k)
				{
					u=i*n+j;
					a[u]=a[u]-a[i*n+k]*a[k*n+j];
				}
				
	for (i=0; i<=n-1; i++)
		if (i!=k)
		{
			u=i*n+k;
			a[u]=-a[u]*a[l];
		}
	} 

	for (k=n-1; k>=0; k--)
	{
		if (js[k]!=k)
			for (j=0; j<=n-1; j++)
			{
				u=k*n+j; v=js[k]*n+j;
				p=a[u]; a[u]=a[v]; a[v]=p; 
			}
			
		if (is[k]!=k)
			for (i=0; i<=n-1; i++)
			{
				u=i*n+k;
				v=i*n+is[k];				
				p=a[u]; a[u]=a[v]; a[v]=p;
			}
	}
			
		free(is);
		free(js);
		
		return(1);
} 

//
//---------------------------------------------------------------------------------------------------------------------
//
double * MatrixInver(double A[],int m,int n) /*矩阵转置*/
    { 
      int i,j; 
      double *B=NULL; 
      B=(double *)malloc(m*n*sizeof(double)); 
      
	  for(i=0;i<n;i++)
		  for(j=0;j<m;j++)
			  B[i*m+j]=A[j*n+i];

      return B; 
     }
//
//
//
double Surplus(double A[],int m,int n) /*求矩阵行列式*/
     { 
      int i,j,k,p,r; 
      double X,temp=1,temp1=1,s=0,s1=0; 
      
	  if(n==2)
      {
		  for(i=0;i<m;i++)
			  for(j=0;j<n;j++)
				  if((i+j)%2) temp1*=A[i*n+j];
				  else temp*=A[i*n+j];
				  X=temp-temp1;
	  } 
      else
	  { 
      
		  for(k=0;k<n;k++)
		  {
			  for(i=0,j=k;i<m,j<n;i++,j++)
				  temp*=A[i*n+j];
			  if(m-i)
			  {
				  for(p=m-i,r=m-1;p>0;p--,r--)
					  temp*=A[r*n+p-1];
			  }
			  
			  s+=temp;
			  temp=1;
		  }
		  
		  for(k=n-1;k>=0;k--)
		  {
			  for(i=0,j=k;i<m,j>=0;i++,j--)
				  temp1*=A[i*n+j];
			  if(m-i)
			  {
				  for(p=m-1,r=i;r<m;p--,r++)
					  temp1*=A[r*n+p];
			  }
			  
			  s1+=temp1;
			  temp1=1;
		  }
		  
		  X=s-s1;
	  }
      
	  return X;
	 
	 }
//
//
//
double * MatrixOpp(double A[],int m,int n) /*矩阵求逆*/ 
     { 
      int i,j,x,y,k; 
      double *SP=NULL,*AB=NULL,*B=NULL,X,*C; 
      SP=(double *)malloc(m*n*sizeof(double)); 
      AB=(double *)malloc(m*n*sizeof(double)); 
      B=(double *)malloc(m*n*sizeof(double)); 
      X=Surplus(A,m,n); 
      X=1/X; 
      for(i=0;i<m;i++) 
      for(j=0;j<n;j++) 
      {for(k=0;k<m*n;k++) 
      B[k]=A[k]; 
      {for(x=0;x<n;x++) 
      B[i*n+x]=0; 
      for(y=0;y<m;y++) 
      B[m*y+j]=0; 
      B[i*n+j]=1; 
      SP[i*n+j]=Surplus(B,m,n); 
      AB[i*n+j]=X*SP[i*n+j]; 
      } 
      } 
      C=MatrixInver(AB,m,n); 
      return C; 
     } 
//
//
//