File content as of revision 0:c55310328157:

#include "mbed.h"
#include "stdio.h"
#include "kalmanfilter.h"
kalmanfilter kalman_filter;

int main() {
     /*----------------------------------------------*/
        /*------              Kalman:KF3       ---------*/
        /*----------------------------------------------*/
    double Tint=0.5; //步长
    double qq=20.0;  //状态噪声;
    double r=15.0*15.0;//量测噪声
    double x0=0;     //初角度估计
    double v0=20.0;     //初始角速度估计
    //double z[]={ 9.2720,  13.513,  6.1300,  35.376,  76.399,  53.196,  71.301,  61.941,  94.043,  100.70,  112.13,  138.60,  128.03,  185.08,  159.94,  169.56,  191.93,  177.63,  206.30,  212.89,  183.21,  259.21,  233.50,  282.24,  264.88,  238.05,  301.91,  323.77,  324.30,  349.54,  341.17,  357.74,  387.27,  341.74,  329.06,  377.47,  306.21,  299.88,  287.88,  269.47,  268.20,  295.22,  198.59,  230.01,  267.01,  197.88,  182.84,  199.56,  147.22,  141.72,  141.17,  111.40,  139.92,  108.83,  77.893,  89.428,  63.567,  71.060,  52.668,  38.430,  12.085, -.90100,  24.456,  8.9650,  48.318,  24.895,  77.915,  68.884,  38.276,  102.70,  83.452,  165.39,  131.92,  131.94,  162.00,  148.20,  170.82,  182.15,  157.87,  214.37,  228.80,  264.62,  234.54,  248.40,  279.18,  288.44,  284.99,  288.35,  260.88,  386.52,  343.78,  339.11,  360.26,  374.39,  342.58,  314.23,  325.68,  318.40,  276.20,  271.67,  284.77,  249.61,  252.33,  217.99,  233.09,  268.20,  121.97,  172.35,  171.80,    180.,  158.09,  140.76,  132.06,  120.83,  89.714,  114.43,  75.168,  84.507,  132.58,  6.2840,  37.705,  27.114, -69.126,  13.381, -5.0320,  11.470,  85.737,  48.845,  30.234,  48.604, -2.5710,  142.25,  108.70,  103.38,  110.79,  68.467,  159.41,  166.86,  174.31,  167.37,  119.62,  271.54,  220.01,  229.39,  221.68,  174.84,  319.50,  274.70,  269.74,  299.71,  282.64,  352.31,  324.82,  300.52,  424.20,  356.93,  368.81,  352.97,  318.94,  338.38,  320.71,  306.19,  278.28,  264.44,  283.42,  234.25,  230.12,  244.88,  227.77,  212.85,  191.38,  192.90,  248.25,  155.24,  150.95,  196.96,  73.301,  112.10,  150.75,  30.608,  74.905,  110.57, -3.6260,  39.705,  83.189, -23.950,  10.042,  16.216,  31.992,  15.359,  16.260,  15.623,  16.699,  15.513,  33.552,  15.359,  15.601};
    //int lengthz=197;
    double z[]={1.659,17.561,31.413,43.155,67.151,105.516,103.745,132.845,138.935,162.869,161.13,205.885,218.191,232.747,275.592,268.136,371.82,298.458,328.422,319.787,369.346,311.032,317.027,292.319,255.571,267.894,216.225,223.599,198.355,175.846,173.86,126.815,124.489,140.198,64.702,72.812,46.116,47.203,-18.359,11.883,37.706,54.226,58.315,61.125,118.907,117.955,156.83,137.986,177.09,205.236,214.884,222.625,208.478,277.71,268.287,308.305,337.767,373.236,367.02,372.394,340.542,323.503,324.125,270.119,313.02,201.609,225.589,232.815,185.559,190.431,114.25,130.986,131.814,110.624,86.737,54.18,65.865,-20.491,1.34,-13.512,78.877,54.287,80.24,79.791,86.722,105.775,132.421,159.287,167.547,191.528,221.675,243.396,244.226,238.432,284.327,295.521,321.958,327.083,358.257};
    int lengthz=99;


    int i;
    FILE* fout;
    double Zk[1];
    double Phik_[3][3];
    double Gamak_1[]={1.0000,    0.0000,    0.0000,
        0.0000,    1.0,    0.0000,
        0.0000,    0.0000,    1.000};
    double Hk[]={1.0000,   0.0, 0.0};
    double Qk_1[3][3];
    double Rk[1][1];
    double Pk_1[]={  .12960e7,        0.,        0.,
        0., .12960e-1,        0.,
        0.,        0., .12960e-4};
    double Xk_1[3];
    double Bk_1[3][1];
    double Uk_1[]={0.0};
    double Xk[3];
    double Pk[3*3];
    double Kk[3*1];
    double Pk_[3*3];
    double Xk_[3];
    int n=3;int m=1;
    int q=3;int p=1;

    Phik_[0][0]=1.0; Phik_[0][1]=Tint;  Phik_[0][2]=Tint*Tint/2.0;
    Phik_[1][0]=0.0; Phik_[1][1]=1.0;   Phik_[1][2]=Tint;
    Phik_[2][0]=0.0; Phik_[2][1]=0.0;   Phik_[2][2]=1.0;

    Qk_1[0][0]=pow(Tint,6)/36.0*qq; Qk_1[0][1]=pow(Tint,5)/12.0*qq; Qk_1[0][2]=pow(Tint,4)/6.0*qq;
    Qk_1[1][0]=pow(Tint,5)/12.0*qq; Qk_1[1][1]=pow(Tint,4)/4.0*qq;  Qk_1[1][2]=pow(Tint,3)/2.0*qq;
    Qk_1[2][0]=pow(Tint,4)/6.0*qq;  Qk_1[2][1]=pow(Tint,3)/2.0*qq;  Qk_1[2][2]=pow(Tint,2)*qq;
    Rk[0][0]=r;
    Xk_1[0]=x0; Xk_1[1]=v0; Xk_1[2]=0;
    Bk_1[0][0]=pow(Tint,3)/6.0;Bk_1[1][0]=pow(Tint,2)/2.0;Bk_1[2][0]=Tint;

    //输出
    fout=fopen("KF3.txt","w");
    for (i=0;i<=lengthz;i++){
        Zk[0]=z[i];
         kalman_filter.Kalman1((Mat)Phik_, (Mat)Gamak_1, (Mat)Hk, (Mat)Qk_1, (Mat)Rk, (Mat)Pk_1, (Vec)Xk_1, (Vec)Zk, (Mat)Bk_1, (Vec)Uk_1,
            n, m, q, p, (Vec)Xk, (Mat)Pk, (Mat)Kk, (Mat)Pk_, (Vec)Xk_);

         kalman_filter.VecCpy((Vec)Xk_1,(Vec)Xk,n);
         kalman_filter.VecCpy((Vec)Pk_1,(Vec)Pk,n*n);

        printf("%.5f\n",Xk[0]);
        fprintf(fout,"%.5f\n",Xk[0]);



    }
    fclose(fout);
    
    }