收藏
下载资源下载资源 加入VIP,免费下载

Kalman滤波算法.docx

上传人：b****4 文档编号：26742634 上传时间：2023-06-22 格式：DOCX 页数：16 大小：25.75KB
下载 相关 举报
Kalman滤波算法.docx_第1页
第1页 / 共16页
Kalman滤波算法.docx_第2页
第2页 / 共16页
Kalman滤波算法.docx_第3页
第3页 / 共16页
Kalman滤波算法.docx_第4页
第4页 / 共16页
Kalman滤波算法.docx_第5页
第5页 / 共16页
点击查看更多>>
下载资源
资源描述

Kalman滤波算法.docx

《Kalman滤波算法.docx》由会员分享，可在线阅读，更多相关《Kalman滤波算法.docx（16页珍藏版）》请在冰豆网上搜索。

Kalman滤波算法.docx

Kalman滤波算法

Kalman滤波算法

姓名：

刘金强

专业：

控制理论与控制工程

学号：

2007255

◆实验目的：

（1）、掌握klman滤波实现的原理和方法

（2）、掌握状态向量预测公式的实现过程

（3）、了解Riccati差分方程实现的过程和新息的基本性质和过程的计算

◆实验要求：

问题：

F=[a1,a2,a3],其中a1=[1.000]的转置，a2=[0.31.00]的转置，a3=[0.10.20.4]的转置，x（0）=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.30.5]的转置，b2=[1,0.4]的转置，b3=[0.8-0.7]的转置；V1（n）=[00n1（n）sin（0.1n）]的转置，V2（n）=[n2（n）n3（n）];n1（n）为均值为零，方差为1的均匀分布白噪声;n2（n）,n3（n）为均值为0，方差为0.1的均匀分布白噪声,n1（n）,n2（n）,n3（n）相互独立，试用卡尔曼滤波器算法估计x^（n）.

◆实验原理：

初始条件：

=E{x

（1）}

K（1,0）=E{[x

（1）-

][x

（1）-

]},其中

=E{x

（1）}

输入观测向量过程：

观测向量序列={y

（1）,…………y（n）}

已知参数：

状态转移矩阵F（n+1,n）

观测矩阵C（n）

过程噪声向量的相关矩阵

观测噪声向量的相关矩阵

计算：

n=1,2,3,……………….

G（n）=F（n+1,n）K（n,n+1）

Kalman滤波器是一种线性的离散时间有限维系统。

Kalman滤波器的估计性能是：

它使滤波后的状态估计误差的相关矩阵P（n）的迹最小化。

这意味着，kalman滤波器是状态向量x（n）的线性最小方差估计。

◆实验结果：

◆程序代码：

（1）主程序

/********************************************************************

问题：

F=[a1,a2,a3],其中a1=[1.000]的转置，a2=[0.31.00]的转置，

a3=[0.10.20.4]的转置，x（0）=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.30.5]的转置，b2=[1,0.4]的转置，b3=[0.8-0.7]的转置；V1（n）=[00n1（n）sin（0.1n）]的转置，V2（n）=[n2（n）n3（n）];n1（n）为均值为零，方差为1的均匀分布白噪声;n2（n）,n3（n）为均值为0，方差为0.1的均匀分布白噪声,n1（n）,n2（n）,n3（n）相互独立，试用卡尔曼滤波器算法估计x^（n）.

*********************************************************************设计作者：

刘金强

设计时间：

2008年11月22日

作者专业：

控制理论与控制工程

作者学号：

2007255

********************************************************************/

#include

#include"MathMatrix.h"

#include

#include

voidGaussInverse（double**a,intn）;

#defineNUM80//最大序列数

voidmain（）

{

CMathMatrixMM;

inti;

intj,w;

doubleF[3][3]={1.0,0.3,0.1,0,0.1,0.2,0,0,0.4};//状态转移矩阵

doubleF_1[3][3]={1.0,0.3,0.1,0,0.1,0.2,0,0,0.4};//状态转移矩阵的逆

double**FN=newdouble*[3];//申请内存空间

for（i=0;i<3;i++）

{

FN[i]=newdouble[3];//开辟一个二维数组

}

for（i=0;i<3;i++）

{

for（j=0;j<3;j++）

{

FN[i][j]=F_1[i][j];

}

}

GaussInverse（FN,3）;

for（i=0;i<3;i++）

{

for（j=0;j<3;j++）

{

F_1[i][j]=FN[i][j];

}

}

doubleF_T[3][3]={1.0,0,0,0.3,1.0,0,0.1,0.2,0.4};//状态转移矩阵的转置

doublex[3]={3,-1,2};//状态向量

doublex1[3]={0,0,0};//状态预测估计向量

doubleC[2][3]={0.3,1,0.8,0.5,0.4,-0.7};//观测矩阵

doubleC_T[3][2]={0.3,0.5,1,0.4,0.8,-0.7};//观测矩阵的转置

doublealpha[2]={0};//新息

doublev1[3]={0};//过程噪声

doubleQ1[3][3]={0};//过程噪声相关矩阵

doublev2[2]={0};//观测噪声

doubleQ2[2][2]={0};//观测噪声相关矩阵

doubley[NUM][2]={0};//观测值序列

doublexx[3][NUM+1]={0};//状态向量序列

doubleK[3][3]={1,0,0,0,1,0,0,0,1};//误差相关矩阵

doubleG[3][2]={0};//卡尔曼增益

doubleP[3][3]={0};//滤波状态向量的误差向量

doublen[NUM*2]={0};

doublen1[NUM]={0};//噪声序列n1,n2,n3

doublen2[NUM]={0};

doublen3[NUM]={0};

doubleM_3x1[3]={0};

doubleM_1x3[3]={0};

doubleM_3x3[3][3]={0};

doubleM1_3x3[3][3]={0};

doubleM_3x2[3][2]={0};

doubleM_2x3[2][3]={0};

doubleM_2x2[2][2]={0};

double**MMid=newdouble*[2];

for（i=0;i<2;i++）

{

MMid[i]=newdouble[2];

}

doubleM_2x1[2]={0};

doublereal_value[3][NUM]={0};

doublepredict_value[3][NUM]={0};

MM.random（NUM,n1,0,1）;//产生高斯白噪声

MM.random（NUM*2,n,0,0.2）;//产生高斯白噪声

memcpy（&n2[1],n,（NUM-1）*8）;

memcpy（&n3[1],n+NUM-1,（NUM-1）*8）;

real_value[0][1]=x[0];

real_value[1][1]=x[1];

real_value[2][1]=x[2];

for（i=1;i

{

v2[0]=n2[i];

v2[1]=n3[i];

v1[0]=0;

v1[1]=0;

v1[2]=n1[i]*sin（0.1*（double）i）;

real_value[0][i]=x[0];

real_value[1][i]=x[1];

real_value[2][i]=x[2];

MM.Maxtrix_multiply（&C[0][0],2,3,&x[0],3,1,&M_2x1[0]）;

MM.Maxtrix_add（&v2[0],&M_2x1[0],2,1）;

MM.Maxtrix_add（&M_2x1[0],&y[i][0],2,1）;

MM.Maxtrix_multiply（&F[0][0],3,3,&x[0],3,1,&M_3x1[0]）;

MM.Maxtrix_add（&v1[0],&M_3x1[0],3,1）;

x[0]=0;x[1]=0;x[2]=0;

MM.Maxtrix_add（&M_3x1[0],&x[0],3,1）;

}

for（i=1;i

{

//计算G（n）

MM.Maxtrix_multiply（&F[0][0],3,3,&K[0][0],3,3,&M_3x3[0][0]）;

MM.Maxtrix_multiply（&M_3x3[0][0],3,3,&C_T[0][0],3,2,&M_3x2[0][0]）;

MM.Maxtrix_multiply（&C[0][0],2,3,&K[0][0],3,3,&M_2x3[0][0]）;

MM.Maxtrix_multiply（&M_2x3[0][0],2,3,&C_T[0][0],3,2,&M_2x2[0][0]）;

v2[0]=n2[i];

v2[1]=n3[i];

MM.Maxtrix_multiply（&v2[0],2,1,&v2[0],1,2,&Q2[0][0]）;

MM.Maxtrix_add（&Q2[0][0],&M_2x2[0][0],2,2）;

MMid[0][0]=M_2x2[0][0];

MMid[0][1]=M_2x2[0][1];

MMid[1][0]=M_2x2[1][0];

MMid[1][1]=M_2x2[1][1];

GaussInverse（MMid,2）;

M_2x2[0][0]=MMid[0][0];

M_2x2[0][1]=MMid[0][1];

M_2x2[1][0]=MMid[1][0];

M_2x2[1][1]=MMid[1][1];

MM.Maxtrix_multiply（&M_3x2[0][0],3,2,&M_2x2[0][0],2,2,&G[0][0]）;

//计算α（n）

MM.Maxtrix_multiply（&C[0][0],2,3,&x1[0],3,1,&M_2x1[0]）;

MM.Maxtrix_sub（&y[i][0],&M_2x1[0],&alpha[0],2,1）;

//估计x1

predict_value[0][i]=x1[0];

predict_value[1][i]=x1[1];

predict_value[2][i]=x1[2];

MM.Maxtrix_multiply（&F[0][0],3,3,&x1[0],3,1,&M_3x1[0]）;

MM.Maxtrix_multiply（&G[0][0],3,2,&alpha[0],2,1,&x1[0]）;

MM.Maxtrix_add（&M_3x1[0],&x1[0],3,1）;

cout<<"估计第"<

"<

cout<

//计算P（n）

MM.Maxtrix_multiply（&F_1[0][0],3,3,&G[0][0],3,2,&M_3x2[0][0]）;

MM.Maxtrix_multiply（&M_3x2[0][0],3,2,&C[0][0],2,3,&M_3x3[0][0]）;

MM.Maxtrix_multiply（&M_3x3[0][0],3,3,&K[0][0],3,3,&M1_3x3[0][0]）;

MM.Maxtrix_sub（&K[0][0],&M1_3x3[0][0],&P[0][0],3,3）;

//生成Q1（n）

v1[0]=0;

v1[1]=0;

v1[2]=n1[i]*sin（0.1*（double）i）;

MM.Maxtrix_multiply（&v1[0],3,1,&v1[0],1,3,&Q1[0][0]）;

//计算K（n）

MM.Maxtrix_multiply（&F[0][0],3,3,&P[0][0],3,3,&M_3x3[0][0]）;

MM.Maxtrix_multiply（&M_3x3[0][0],3,3,&F_T[0][0],3,3,&K[0][0]）;

MM.Maxtrix_add（&Q1[0][0],&K[0][0],3,3）;

}

//释放内存

for（i=0;i<3;i++）

{

delete[]FN[i];

}

delete[]FN;

for（i=0;i<2;i++）

{

delete[]MMid[i];

}

delete[]MMid;

}

（2）CMathMatrix文件的源文件CMathMatrix.cpp：

//mathMatrix.cpp:

implementationoftheCmathMatrixclass.

///////////////////////////////////////////////////////////////////

#include"mathMatrix.h"

#include

#include

#include

#include

#include

#defineSWAP（a,b）{temp=（a）;（a）=（b）;（b）=temp;}

///////////////////////////////////////////////////////////////////

//Construction/Destruction

///////////////////////////////////////////////////////////////////

CMathMatrix:

:

CMathMatrix（）

{

}

CMathMatrix:

:

~CMathMatrix（）

{

}

//矩阵求逆

int*ivector（longnl,longnh）;

voidnrerror（charerror_text[]）;

voidfree_ivector（int*v,longnl,longnh）;

voidGaussInverse（double**a,intn）

{

int*indxc,*indxr,*ipiv;

inti,icol,irow,j,k,l,ll;

doublebig,dum,pivinv,temp;

indxc=ivector（1,n）;

indxr=ivector（1,n）;

ipiv=ivector（1,n）;

for（j=1;j<=n;j++）ipiv[j]=0;

for（i=1;i<=n;i++）

{

big=0.0;

for（j=1;j<=n;j++）

if（ipiv[j]!

=1）

for（k=1;k<=n;k++）

{

if（ipiv[k]==0）

{

if（fabs（a[j-1][k-1]）>=big）

{

big=fabs（a[j-1][k-1]）;

irow=j;

icol=k;

}

}

elseif（ipiv[k]>1）

nrerror（"gaussj:

SingularMatrix-1"）;

}

++（ipiv[icol]）;

if（irow!

=icol）

{

for（l=1;l<=n;l++）

SWAP（a[irow-1][l-1],a[icol-1][l-1]）;

}

indxr[i]=irow;

indxc[i]=icol;

if（a[icol-1][icol-1]==0.0）

nrerror（"gaussj:

SingularMatrix-2"）;

pivinv=1.0/a[icol-1][icol-1];

a[icol-1][icol-1]=1.0;

for（l=1;l<=n;l++）

a[icol-1][l-1]*=pivinv;

for（ll=1;ll<=n;ll++）

if（ll!

=icol）

{

dum=a[ll-1][icol-1];

a[ll-1][icol-1]=0.0;

for（l=1;l<=n;l++）

a[ll-1][l-1]-=a[icol-1][l-1]*dum;

}

}

for（l=n;l>=1;l--）

{

if（indxr[l]!

=indxc[l]）

for（k=1;k<=n;k++）

SWAP（a[k-1][indxr[l]-1],a[k-1][indxc[l]-1]）;

}

free_ivector（ipiv,1,n）;

free_ivector（indxr,1,n）;

free_ivector（indxc,1,n）;

}

#undefSWAP

#undefNRANSI

//生成高斯白噪声

voidCMathMatrix:

:

random（intn,double*e,floatmean,floatvar）

{

longj;

floata,b,tt;

srand（（unsigned）time（NULL））;

for（j=0;j

{

tt=rand（）;

if（（tt-0.000001）<0）

{

j--;

continue;

}

a=sqrt（-2.0*log（tt/32767.0））;

b=2*3.*rand（）/32767.0;

e[j]=var*a*cos（b）+mean;

}

}

//矩阵相加

voidCMathMatrix:

:

Maxtrix_add（double*Matrix_A,double*Matrix_B,unsignedintrow,unsignedintcol）

{

unsignedinti,j;

for（i=0;i

for（j=0;j

Matrix_B[i*col+j]+=Matrix_A[i*col+j];

}

//矩阵相减

voidCMathMatrix:

:

Maxtrix_sub（double*Matrix_A,double*Matrix_B,double*Matrix_C,unsignedintrow,unsignedintcol）

{

unsignedinti,j;

for（i=0;i

for（j=0;j

Matrix_C[i*col+j]=Matrix_A[i*col+j]-Matrix_B[i*col+j];

}

//矩阵相乘

voidCMathMatrix:

:

Maxtrix_multiply（double*Matrix_A,unsignedintA_row,unsignedintA_col,double*Matrix_B,unsignedintB_row,unsignedintB_col,double*Matrix_out）

{

unsignedinti,j,k;

doublesum=0;

for（i=0;i

for（j=0;j

{

sum=0;

for（k=0;k

sum+=Matrix_A[i*A_col+k]*Matrix_B[k*B_col+j];

Matrix_out[i*B_col+j]=sum;

}

}

（3）、CMathMatrix文件的头文件CMathMatrix.h:

//MathMatrix.h:

interfacefortheCMathMatrixclass.

//

//////////////////////////////////////////////////////////////////////

#if!

defined（AFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__INCLUDED_）

#defineAFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__INCLUDED_

#if_MSC_VER>1000

#pragmaonce

#endif//_MSC_VER>1000

classCMathMatrix

{

public:

//生成高斯白噪声

voidrandom（intn,double*e,floatmean,floatvar）;

//矩阵相加

voidCMathMatrix:

:

Maxtrix_add（double*Matrix_A,double*Matrix_B,unsignedintrow,unsignedintcol）;

//矩阵相减

voidCMathMatrix:

:

Max

展开阅读全文
相关资源
猜你喜欢
相关搜索
资源标签

当前位置：首页 > 党团工作 > 入党转正申请

copyright@ 2008-2022 冰豆网网站版权所有

经营许可证编号:鄂ICP备2022015515号-1