|
|
楼主 |
发表于 2005-1-19 22:57:19
|
显示全部楼层
< >/离散随机线性系统的卡尔曼（kalman）滤波
//n-整型变量，动态系统的维数
//m-整型变量，观测系统的维数
//k-观测序列的长度
//f-n*n数组，系统状态转移矩阵
//q-n*n数组，模型噪声Wk的协方差矩阵
//r-m*m数组，观测噪声Vk的协方差矩阵
//h-m*n数组，观测矩阵
//yy-k*m数组，观测向量序列
//x-k*n数组，x[0,j]存放给定的初值，其余各行返回状态向量估计序列
//p-n*n数组，存放初值P0,返回时存放最后时刻的估计误差协方差矩阵
//g-n*m数组，返回最后时刻的稳定增益矩阵
int brinv(double a[],int n);
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;
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=brinv(e,m);
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);
}</P><>int brinv(double a[],int n)
{
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;
a=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;
a=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=a*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=a-a[i*n+k]*a[k*n+j];
}
}
}
}
for (i=0; i<=n-1; i++)
{
if (i!=k)
{
u=i*n+k;
a=-a*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;
a=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;
a=a[v];
a[v]=p;
}
}
}
free(is); free(js);
return(1);
}</P> |
|
发表于 2005-1-19 22:56:15