Kalman 滤波算法姓名：刘金强专业：控制理论与控制工程学号：2007255◆实验目的：（1）、掌握klman 滤波实现的原理和方法（2）、掌握状态向量预测公式的实现过程（3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求：问题：F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(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).◆实验原理：初始条件：1ˆ(1)x=E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)}输入观测向量过程：观测向量序列={y(1),…………y(n)}已知参数：状态转移矩阵F(n+1,n)观测矩阵C(n)过程噪声向量的相关矩阵1()Q n观测噪声向量的相关矩阵2()Q n计算：n=1,2,3,……………….G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+Kalman 滤波器是一种线性的离散时间有限维系统。

Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。

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

◆实验结果：◆程序代码：（1）主程序/******************************************************************** 问题：F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(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 <iostream.h>#include "MathMatrix.h"#include <math.h>#include <string.h>void GaussInverse(double **a, int n);#define NUM 80 //最大序列数void main(){CMathMatrix MM;int i;int j, w;double F[3][3] = {1.0,0.3,0.1, 0, 0.1,0.2,0, 0, 0.4}; //状态转移矩阵double F_1[3][3] = {1.0,0.3,0.1,0, 0.1,0.2,0, 0, 0.4}; //状态转移矩阵的逆double **FN = new double * [3]; //申请内存空间for (i = 0; i < 3; i++){FN[i] = new double [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];}}double F_T[3][3] = {1.0, 0, 0,0.3,1.0, 0,0.1,0.2,0.4}; //状态转移矩阵的转置double x[3] = {3,-1,2}; //状态向量double x1[3] = {0,0,0}; //状态预测估计向量double C[2][3] = {0.3, 1, 0.8,0.5,0.4,-0.7}; //观测矩阵double C_T[3][2] = {0.3,0.5,1, 0.4, 0.8,-0.7}; //观测矩阵的转置double alpha[2]={0}; //新息double v1[3]={0}; //过程噪声double Q1[3][3]={0}; //过程噪声相关矩阵double v2[2]={0}; //观测噪声double Q2[2][2]={0}; //观测噪声相关矩阵double y[NUM][2]={0}; //观测值序列double xx[3][NUM+1]={0}; //状态向量序列double K[3][3]={1,0,0,0,1,0,0,0,1}; //误差相关矩阵double G[3][2]={0}; //卡尔曼增益double P[3][3]={0}; //滤波状态向量的误差向量double n[NUM*2]={0};double n1[NUM]={0}; //噪声序列n1,n2,n3double n2[NUM]={0};double n3[NUM]={0};double M_3x1[3]={0};double M_1x3[3]={0};double M_3x3[3][3]={0};double M1_3x3[3][3]={0};double M_3x2[3][2]={0};double M_2x3[2][3]={0};double M_2x2[2][2]={0};double **MMid = new double *[2];for (i = 0; i < 2; i++){MMid[i] = new double [2];}double M_2x1[2]={0};double real_value[3][NUM]={0};double predict_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<NUM;i++) //生成观测序列y{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<NUM;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);//估计x1predict_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<<"估计第"<<i<<"次的值为："<<endl;cout<<x[0]<<" "<<x1[0]<<endl<<x[1]<<" "<<x1[1]<<endl<<x[2]<<" "<<x1[2]<<endl;//计算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: implementation of the CmathMatrix class./////////////////////////////////////////////////////////////////// #include "mathMatrix.h"#include <time.h>#include <math.h>#include <stdio.h>#include <stdlib.h>#include <iostream.h>#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}/////////////////////////////////////////////////////////////////// // Construction/Destruction/////////////////////////////////////////////////////////////////// CMathMatrix::CMathMatrix(){}CMathMatrix::~CMathMatrix(){}//矩阵求逆int *ivector(long nl, long nh);void nrerror(char error_text[]);void free_ivector(int *v, long nl, long nh);void GaussInverse(double **a, int n){int *indxc, *indxr, *ipiv;int i, icol, irow, j, k, l, ll;double big, 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;}}else if ( ipiv[k] > 1)nrerror("gaussj: Singular Matrix-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: Singular Matrix-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);}#undef SWAP#undef NRANSI//生成高斯白噪声void CMathMatrix::random(int n, double *e, float mean, float var) {long j;float a, b, tt;srand((unsigned)time(NULL));for(j = 0; j < n; 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;}}//矩阵相加void CMathMatrix::Maxtrix_add(double *Matrix_A, double *Matrix_B, unsigned int row, unsigned int col){unsigned int i,j;for(i=0;i<row;i++)for(j=0;j<col;j++)Matrix_B[i*col+j] += Matrix_A[i*col+j];}//矩阵相减void CMathMatrix::Maxtrix_sub(double *Matrix_A, double *Matrix_B, double *Matrix_C, unsigned int row, unsigned int col){unsigned int i,j;for(i=0;i<row;i++)for(j=0;j<col;j++)Matrix_C[i*col+j] = Matrix_A[i*col+j]-Matrix_B[i*col+j];}//矩阵相乘void CMathMatrix::Maxtrix_multiply(double *Matrix_A, unsigned intA_row, unsigned int A_col, double *Matrix_B, unsigned int B_row, unsigned int B_col, double *Matrix_out){unsigned int i,j,k;double sum = 0;for(i=0;i<A_row;i++)for(j=0;j<B_col;j++){sum = 0;for(k=0;k<A_col;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: interface for the CMathMatrix class./////////////////////////////////////////////////////////////////////// /#if !defined(AFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__ INCLUDED_)#defineAFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__INCLUDED_#if _MSC_VER > 1000#pragma once#endif // _MSC_VER > 1000class CMathMatrix{public://生成高斯白噪声void random(int n, double *e, float mean, float var);//矩阵相加void CMathMatrix::Maxtrix_add(double *Matrix_A, double *Matrix_B, unsigned int row, unsigned int col);//矩阵相减void CMathMatrix::Maxtrix_sub(double *Matrix_A, double *Matrix_B, double *Matrix_C, unsigned int row, unsigned int col);//矩阵相乘void CMathMatrix::Maxtrix_multiply(double *Matrix_A, unsigned int A_row, unsigned int A_col, double *Matrix_B, unsigned int B_row, unsigned int B_col, double *Matrix_out);//矩阵求逆void GaussInverse(double **a, int n);//默认构造函数与析构函数CMathMatrix();virtual ~CMathMatrix();};#endif// !defined(AFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__I NCLUDED_)备注：矩阵高斯求逆矩阵的函数GaussInverse的nrutil.cpp文件和nrutil.h文档来源为:从网络收集整理.word版本可编辑.欢迎下载支持. 文件在此省略。