dlgkalman.cpp

该软件是我读硕士的时候写的

// DlgKalman.cpp : implementation file
//

#include "stdafx.h"
#include "process.h"
#include "DlgKalman.h"

#include "Matrix1.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// DlgKalman dialog


DlgKalman::DlgKalman(CWnd* pParent /*=NULL*/)
	: CDialog(DlgKalman::IDD, pParent)
{
	//{{AFX_DATA_INIT(DlgKalman)
	m_x1 = 60.0;
	m_x2 = 0.0;
	m_q1 = 0.0;
	m_q2 = 0.01;
	m_R = 0.01;
	m_T = 0.1;
	//}}AFX_DATA_INIT
}


void DlgKalman::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(DlgKalman)
	DDX_Text(pDX, IDC_EDIT7, m_x1);
	DDX_Text(pDX, IDC_EDIT8, m_x2);
	DDX_Text(pDX, IDC_EDIT2, m_q1);
	DDX_Text(pDX, IDC_EDIT6, m_q2);
	DDX_Text(pDX, IDC_EDIT3, m_R);
	DDX_Text(pDX, IDC_EDIT1, m_T);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(DlgKalman, CDialog)
	//{{AFX_MSG_MAP(DlgKalman)
	ON_BN_CLICKED(IDC_BUTTON1, OnKalman)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// DlgKalman message handlers

void DlgKalman::OnKalman() 
{
//kaishi lvbo 
	int i;
	
	double Kdouble;
	double tmpx1,tmpx2;

	UpdateData(true);
	T=m_T;
	q1=m_q1;
	q2=m_q2;
	R=m_R;
	x1=m_x1;
	x2=m_x2;
	
	CMatrix1 X00(2,1),X10(2,1),X11(2,1);
	CMatrix1 P00(2,2),P10(2,2),P11(2,2);
    //状态初值
	X00.SetElement(0,0,x1);
	X00.SetElement(1,0,x2);
    //误差初值
	P00.SetElement(0,0,0.0);
	P00.SetElement(0,1,0.0);
	P00.SetElement(1,0,0.0);
	P00.SetElement(1,1,0.0);

	CMatrix1 A(2,2);
	CMatrix1 f(2,1);
	CMatrix1 F(2,2);
	
	double tmp1[60],tmpx[60];
	CMatrix1 E;
	CMatrix1 K(2,1),Ktmp;
	E.MakeUnitMatrix(2);

	CMatrix1 H(1,2);
	H.SetElement(0,0,1.0);
	H.SetElement(1,0,0.0);
/*
	tmpH[0]=H.m_pData[0];
	tmpH[1]=H.m_pData[1];
*/

	double tmpQ[4]={q1,0.0,0.0,q2};
	CMatrix1 Q(2,2,tmpQ);

	KalmanData[0]=DataData[0];
 
   //滤波!!!
   for(i=1;i<DataNum;i++)
   {
	   f.SetElement(0,0,X00.m_pData[0]*X00.m_pData[1]);
	   f.SetElement(1,0,0.0);
	   
	   F.SetElement(0,0,X00.m_pData[1]);
	   F.SetElement(0,1,X00.m_pData[0]);
	   F.SetElement(1,0,0.0);
	   F.SetElement(1,1,0.0);
	   

	   tmpx1=X00.m_pData[0];
	   tmpx2=X00.m_pData[1];

//	   tmpA[4]={1+tmpx2*Time+tmpx2*tmpx2*Time*Time*0.5,tmpx1*Time+tmpx1*tmpx2*Time*Time,0.0,1.0};
	  
	   A.SetElement(0,0,1+tmpx2*T+tmpx2*tmpx2*T*T*0.5);
	   A.SetElement(0,1,tmpx1*T+tmpx1*tmpx2*T*T);
	   A.SetElement(1,0,0.0);
	   A.SetElement(1,1,1.0);
/*
	   tmpA[0]=A.m_pData[0];
	   tmpA[1]=A.m_pData[1];
	   tmpA[2]=A.m_pData[2];
	   tmpA[3]=A.m_pData[3];
*/
	 
       X10=X00+f*T+F*f*T*T*0.5;

	   CMatrix1 AT=A.Transpose();
	   P10=A*P00*AT+Q;
	  
	   CMatrix1 HT=H.Transpose();
	   Ktmp=H*P10*HT;
	   Kdouble=Ktmp.m_pData[0];
	   Kdouble=Kdouble+R;
	   Kdouble=1/Kdouble;
	   K=P10*HT*Kdouble;
	   CMatrix1 KT=K.Transpose();
	 	   
	   X11=X10+K*DataData[i]-K*H*X10;
	    
       CMatrix1 Ptmp;
	   Ptmp=E-K*H;
//	   CMatrix1 PtmpT=Ptmp.Transpose();
	   P11=Ptmp*P10;
         
	   X00=X11;
 	   P00=P11;
	   KalmanData[i]=X00.m_pData[0];

    
   }

   /*
   tmpK[0]=K.m_pData[0];
     tmpK[1]=K.m_pData[1];
     
    
     tmpf[0]=f.m_pData[0];
     tmpf[1]=f.m_pData[1];
  
     tmpF[0]=F.m_pData[0];
     tmpF[1]=F.m_pData[1];
     tmpF[2]=F.m_pData[2];
     tmpF[3]=F.m_pData[3];
  
     tmpP[0]=P00.m_pData[0];
     tmpP[1]=P00.m_pData[1];
     tmpP[2]=P00.m_pData[2];
     tmpP[3]=P00.m_pData[3];
     
     tmpX[0]=X00.m_pData[0];
     tmpX[1]=X00.m_pData[1];	// TODO: Add your control notification handler code here
  */
  	
}

void DlgKalman::OnOK() 
{
		
	CDialog::OnOK();
}