yy[n-1]=-y[n-5]+4.0*y[n-4]-6.0*y[n-3];
yy[n-1]=(yy[n-1]+4.0*y[n-2]+69.0*y[n-1])/70.0;
}
return;
}
/离散随机线性系统的卡尔曼（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;
上一篇:在SPLUS中直接连接SQLSERVER 2005
下一篇:《雷神之锤III》里求平方根的函数
|
精品推荐