Shao Rui / Mbed 2 deprecated Kalman

1

Dependencies:   mbed

main.cpp@0:c55310328157, 2020-02-07 (annotated)

Committer:
shaorui
Date:
Fri Feb 07 11:34:24 2020 +0000
Revision:
0:c55310328157
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shaorui 0:c55310328157 1 #include "mbed.h"
shaorui 0:c55310328157 2 #include "stdio.h"
shaorui 0:c55310328157 3 #include "kalmanfilter.h"
shaorui 0:c55310328157 4 kalmanfilter kalman_filter;
shaorui 0:c55310328157 5
shaorui 0:c55310328157 6 int main() {
shaorui 0:c55310328157 7 /*----------------------------------------------*/
shaorui 0:c55310328157 8 /*------ Kalman:KF3 ---------*/
shaorui 0:c55310328157 9 /*----------------------------------------------*/
shaorui 0:c55310328157 10 double Tint=0.5; //步长
shaorui 0:c55310328157 11 double qq=20.0; //状态噪声;
shaorui 0:c55310328157 12 double r=15.0*15.0;//量测噪声
shaorui 0:c55310328157 13 double x0=0; //初角度估计
shaorui 0:c55310328157 14 double v0=20.0; //初始角速度估计
shaorui 0:c55310328157 15 //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};
shaorui 0:c55310328157 16 //int lengthz=197;
shaorui 0:c55310328157 17 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};
shaorui 0:c55310328157 18 int lengthz=99;
shaorui 0:c55310328157 19
shaorui 0:c55310328157 20
shaorui 0:c55310328157 21 int i;
shaorui 0:c55310328157 22 FILE* fout;
shaorui 0:c55310328157 23 double Zk[1];
shaorui 0:c55310328157 24 double Phik_[3][3];
shaorui 0:c55310328157 25 double Gamak_1[]={1.0000, 0.0000, 0.0000,
shaorui 0:c55310328157 26 0.0000, 1.0, 0.0000,
shaorui 0:c55310328157 27 0.0000, 0.0000, 1.000};
shaorui 0:c55310328157 28 double Hk[]={1.0000, 0.0, 0.0};
shaorui 0:c55310328157 29 double Qk_1[3][3];
shaorui 0:c55310328157 30 double Rk[1][1];
shaorui 0:c55310328157 31 double Pk_1[]={ .12960e7, 0., 0.,
shaorui 0:c55310328157 32 0., .12960e-1, 0.,
shaorui 0:c55310328157 33 0., 0., .12960e-4};
shaorui 0:c55310328157 34 double Xk_1[3];
shaorui 0:c55310328157 35 double Bk_1[3][1];
shaorui 0:c55310328157 36 double Uk_1[]={0.0};
shaorui 0:c55310328157 37 double Xk[3];
shaorui 0:c55310328157 38 double Pk[3*3];
shaorui 0:c55310328157 39 double Kk[3*1];
shaorui 0:c55310328157 40 double Pk_[3*3];
shaorui 0:c55310328157 41 double Xk_[3];
shaorui 0:c55310328157 42 int n=3;int m=1;
shaorui 0:c55310328157 43 int q=3;int p=1;
shaorui 0:c55310328157 44
shaorui 0:c55310328157 45 Phik_[0][0]=1.0; Phik_[0][1]=Tint; Phik_[0][2]=Tint*Tint/2.0;
shaorui 0:c55310328157 46 Phik_[1][0]=0.0; Phik_[1][1]=1.0; Phik_[1][2]=Tint;
shaorui 0:c55310328157 47 Phik_[2][0]=0.0; Phik_[2][1]=0.0; Phik_[2][2]=1.0;
shaorui 0:c55310328157 48
shaorui 0:c55310328157 49 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;
shaorui 0:c55310328157 50 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;
shaorui 0:c55310328157 51 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;
shaorui 0:c55310328157 52 Rk[0][0]=r;
shaorui 0:c55310328157 53 Xk_1[0]=x0; Xk_1[1]=v0; Xk_1[2]=0;
shaorui 0:c55310328157 54 Bk_1[0][0]=pow(Tint,3)/6.0;Bk_1[1][0]=pow(Tint,2)/2.0;Bk_1[2][0]=Tint;
shaorui 0:c55310328157 55
shaorui 0:c55310328157 56 //输出
shaorui 0:c55310328157 57 fout=fopen("KF3.txt","w");
shaorui 0:c55310328157 58 for (i=0;i<=lengthz;i++){
shaorui 0:c55310328157 59 Zk[0]=z[i];
shaorui 0:c55310328157 60 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,
shaorui 0:c55310328157 61 n, m, q, p, (Vec)Xk, (Mat)Pk, (Mat)Kk, (Mat)Pk_, (Vec)Xk_);
shaorui 0:c55310328157 62
shaorui 0:c55310328157 63 kalman_filter.VecCpy((Vec)Xk_1,(Vec)Xk,n);
shaorui 0:c55310328157 64 kalman_filter.VecCpy((Vec)Pk_1,(Vec)Pk,n*n);
shaorui 0:c55310328157 65
shaorui 0:c55310328157 66 printf("%.5f\n",Xk[0]);
shaorui 0:c55310328157 67 fprintf(fout,"%.5f\n",Xk[0]);
shaorui 0:c55310328157 68
shaorui 0:c55310328157 69
shaorui 0:c55310328157 70
shaorui 0:c55310328157 71 }
shaorui 0:c55310328157 72 fclose(fout);
shaorui 0:c55310328157 73
shaorui 0:c55310328157 74 }
shaorui 0:c55310328157 75