Kalman滤波算法.doc

文档介绍：Kalman滤波算法姓名:刘金强专业:控制理论与控制工程学号:2007255◆实验目的:(1)、掌握klman滤波实现的原理和方法(2)、掌握状态向量预测公式的实现过程(3)、ati差分方程实现的过程和新息的基本性质和过程的计算◆实验要求:问题:F=[a1,a2,a3],其中a1=[]的转置,a2=[]的转置,a3=[]的转置,x(0)=[3,-1,2]的转置;C=[b1,b2,b3],其中b1=[]的转置,b2=[1,]的转置,b3=[-]的转置;V1(n)=[00n1(n)sin()]的转置,V2(n)=[n2(n)n3(n)];n1(n)为均值为零,方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0,,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=[]的转置,a2=[]的转置,a3=[]的转置,x(0)=[3,-1,2]的转置;C=[b1,b2,b3],其中b1=[]的转置,b2=[1,]的转置,b3=[-]的转置;V1(n)=[00n1(n)sin()]的转置,V2(n)=[n2(n)n3(n)];n1(n)为均值为零,方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0,,n1(n),n2(n),n3(n)相互独立,试用卡尔曼滤波器算法估计x^(n).*********************************************************************设计作者:刘金强设计时间:2008年11月22日作者专业:控制理论与控制工程作者学号:2007255********************************************************************/#include<>#include""#include<>#include<>voidGaussInverse(double**a,intn);#defineNUM80 //最大序列数voidmain(){ CMathMatrixMM; inti; intj,w; doubleF[3][3]={,,,0,,,0,0,};//状态转移矩阵 doubleF_1[3][3]={,,,0,,,0,0,};//状态转移矩阵的逆 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]={,0,0,,,0,,,};//状态转移矩阵的转置 doublex[3]={3,-1,2};//状态向量 doublex1[3]={0,0,0}; //状态预测估计向量 doubleC[2][3]={,1,,,,-};//观测矩阵 doubleC_T[3][2]={,,1,,,-};//观测矩阵的转置 doublealpha[2]={0}; //新息 doubl