UKF C++版本

#include 
#include “matrix.h“ //矩阵操作的头文件
#include “obsfun.h“  //观测方程
#include “sysfun.h“  //系统方城
#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++） //指定滤波次数
            {


                //获得观测值
                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用于下一次循环

                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