#include?
#include?“matrix.h“?//矩陣操作的頭文件
#include?“obsfun.h“??//觀測方程
#include?“sysfun.h“??//系統(tǒng)方城
#include?“sigmapoint.h“?//求sigma點
#include?“ukf.h“???????//ukf濾波算法
#include?
#include?
#include?
using?namespace?std;



/************************************************************************/
/*??????測試????????????????????????????????????????????????????????????*/
/************************************************************************/

int?main（）
{
????clock_t?start?finish;
????start?=?clock（）;

????//假定初始值如下
????Matrix?x0（41）;
????Matrix?p0（44）Q（44）R（22）;
????double?a[4]={-280010016010};
????double?b[16]={100?0?0?0
??????????????????0?100?0?0
??????????????????0?0?100?0
??????????????????0?0?0?100};
????double?c[16]={0.0001?0?0?0
??????????????????0?0.0001?0?0
??????????????????0?0?0.0001?0
??????????????????0?0?0?0.0001};
????double?d[4]={0.0001002500};
????x0=a;p0=b;Q=c;R=d;

????Matrix?measure（250001）kongzhi（250001）;
????measure.Null（）;kongzhi.Null（）;


????ifstream?stream0（“guance.txt“）;
????ifstream?stream1（“kongzhi.txt“）;
????FILE?*stream2;
????stream2=fopen（“result.txt““w+“）;

????if（?!stream0?&&?!stream1?&&?stream2?==?NULL）
????{
????????printf（?“The?file??was?not?opened\n“?）;
????????return?false;
????}

????fprintf（stream2“%f?%20f?%20f?%20f\n“x0（00）x0（10）x0（20）x0（30））;

????int?i=0i1=0i2=0;
????double?temp0=0.0;
????double?temp1=0.0;int?j=0;

????while（?!stream0.eof（）&!stream1.eof（）??）
????{
????????stream0>>temp0;
????????if?（（i+1）%2==0）
????????{?measure（1i1）=temp0;i1++;}
????????else
????????{measure（0i2）=temp0;?i2++;}

????????i++;

????????stream1>>temp1;
????????kongzhi（0j）=temp1;
????????j++;

????}


????Matrix?control（21）guance（21）;
????control.Null（）;guance.Null（）;


/*濾波處理過程********************************************************************/

????TRYBEGIN（）??//?異常處理
????????{
????????????//定義實例
????????????Sigma?sigmapoint（x00.8520）;

????????????UKF?ss（QR）;

????????????for（int?k=0;k<49999;k++）?//指定濾波次數(shù)
????????????{


????????????????//獲得觀測值
????????????????guance（00）=measure（0k+1）;guance（10）=measure（1k+1）;
????????????????control（00）=kongzhi（0k）;


????????????????//先求sigma點
????????????????sigmapoint.SigmaSet（x0p0）;
????????????????sigmapoint.SigmaFun（）;

????????????????//ukf濾波
????????????????ss.UkfSet（sigmapoint.sigmasigmapoint.w_m
??????????????????????????sigmapoint.w_cguancecontrolQR）;
????????????????ss.Process（）;

????????????????p0=ss.p_est;?x0=ss.x_est;??//更新x0p0用于下一次循環(huán)

????????????????fprintf（stream2“%f?%20f?%20f?%20f\n“x0（00）x0（10）x0（20）x0（30））;


????????????}
????????}
????CATCHERROR（）?//異常

????fclose（stream2）;
????stream0.close（）;
????stream1.close（）;
/*********************************************************************************/

????finish?=?clock（）;

????double?duration?=?（double）（finish?-?start）?/?CLOCKS_PER_SEC;
????std::cout<<“程序運行了：“<
????sy