CODEWITHSUNDEEP
matlabpredictionestimationkalman-filter
user3593525
user3593525

Reputation: 253

input position/velocity to kalman filter matlab

I'm trying to track an object i 3-D space where I'v an objects position and directional velocity. I wrote a class in Matlab for that sake, however, the equations/algorithm of my tracking algorithm EKF is working fine as every current and previous states are predicted fine, but, I want to input a trajectory of Nx3 points, i'm getting bug of this.
However I have also the directional velocity's information, i.e derivative of position vector.

All I'm confusing in position estimation/prediction input only position or with velocity, as obj.X(:,1) = [6x1] and also obj.Xh(:,1) = [6x1], is this means [x,y,z,vx,vy,vz] ?

if so, how can I INPUT this to check its estimation, and if not, how can I estimate the POSITION as my goal is only the estimation of position only.

My EKF:

classdef EKF <handle
    properties (Access=private)
        H
        K
        Z
        Q
        M
        ind
        A
        X
        Xh
        P
        a
        b
    end
    methods
        function obj = EKF(position)
            obj.H = [];
            obj.K = [];
            obj.Z  = [];
            obj.ind=0; % indicator function. Used for unwrapping of tan
            obj.Q =[0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0.01 0 0;
                0 0 0 0 0.01 0;
                0 0 0 0 0 0.01];% Covarience matrix of process noise
            obj.M=[0.001 0 0;
                0 0.001 0;
                0 0 0.001]; % Covarience matrix of measurment noise
            obj.A=[1 0 0 0.1 0 0;
                0 1 0 0 0.1 0;
                0 0 1 0 0 0.1;
                0 0 0 1 0 0;
                0 0 0 0 1 0;
                0 0 0 0 0 1]; % System Dynamics

obj.X(:,1)=[position(1,:) position(2,:)];
            obj.Xh(:,1)=[position(1,:) position(2,:)];%Assumed initial conditions
            obj.Z(:,:,1)=position(1,:)';% initial observation
            obj.P(:,:,1)=[0.1 0 0 0 0 0;
                0 0.1 0 0 0 0;
                0 0 0.1 0 0 0;
                0 0 0 0.1 0 0;
                0 0 0 0 0.1 0;
                0 0 0 0 0 0.1]; %inital value of covarience of estimation error

        end

        function [obj,predictedS]=EKFpredictor(obj,p,n)
            function   [ARG]=arctang(a,b,ind)
                if b<0 && a>0 % PLACING IN THE RIGHT QUADRANT
                    ARG=abs(atan(a/b))+pi/2;
                elseif b<0 && a<0
                    ARG=abs(atan(a/b))+pi;
                elseif b>0 && a<0
                    ARG=abs(atan(a/b))+3*pi/2;
                else
                    ARG=atan(a/b);
                end
                if ind==-1 % UNWARPPING PART
                    ARG=ARG-2*pi;
                else
                    if ind==1;
                        ARG=ARG+2*pi;
                    end
                end
            end
              obj.X(:,n-1)=[obj.X(1:3,n-1)' p]'; 

            %% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
            % State process % w generating process noise
            obj.X(:,n)=obj.A*obj.X(:,n-1)+[0;0;0;sqrt(obj.Q(4,4))*randn(1);sqrt(obj.Q(5,5))*randn(1);sqrt(obj.Q(6,6))*randn(1)];
            %generating & observation observation noise
            obj.Z(:,:,n)=[sqrt(obj.X(1,n-1)^2+obj.X(2,n-1)^2);arctang(obj.X(2,n-1),obj.X(1,n-1),obj.ind);obj.X(3,n-1)]+[sqrt(obj.M(1,1))*randn(1);sqrt(obj.M(1,1))*randn(1);sqrt(obj.M(1,1))*randn(1)];
            %% PREDICTION OF NEXT STATE
            % ESTIMATE
            obj.Xh(:,n)=obj.A*obj.Xh(:,n-1);
            predictedS=obj.Xh(:,n)';
            % PRIORY ERROR COVARIENCE
            obj.P(:,:,n)=obj.A*obj.P(:,:,n-1)*obj.A'+obj.Q;
            %% CORRECTION EQUTIONS
            % Jacobian matrix
            obj.H(:,:,n-1)=[obj.Xh(1,n)/(sqrt(obj.Xh(1,n)^2+obj.Xh(2,n)^2)), obj.Xh(2,n)/(sqrt(obj.Xh(1,n)^2+obj.Xh(2,n)^2)),0,0,0,0; ...
                -obj.Xh(2,n)/(sqrt(obj.Xh(1,n)^2+obj.Xh(2,n)^2)), obj.Xh(1,n)/(sqrt(obj.Xh(1,n)^2+obj.Xh(2,n)^2)),0,0,0,0; ...
                0,0,1,0,0,0];
            % Kalman Gain
            obj.K(:,:,n)=obj.P(:,:,n)*obj.H(:,:,n-1)'*(obj.M+obj.H(:,:,n-1)*obj.P(:,:,n)*obj.H(:,:,n-1)')^(-1);
            % INNOVATION
            Inov=obj.Z(:,:,n)-[sqrt(obj.Xh(1,n)^2+obj.Xh(2,n)^2);arctang(obj.Xh(2,n),obj.Xh(1,n),obj.ind);obj.Xh(3,n)];
            %computes final estimate
            obj.Xh(:,n)=obj.Xh(:,n)+ obj.K(:,:,n)*Inov;
            %computes covarience of estimation error
            obj.P(:,:,n)=(eye(6)-obj.K(:,:,n)*obj.H(:,:,n-1))*obj.P(:,:,n);
            %% unwrapping the tan function
            if abs(arctang(obj.Xh(1,n),obj.Xh(2,n),0)-arctang(obj.Xh(1,n-1),obj.Xh(2,n-1),0))>=pi
                if obj.ind==1
                    obj.ind=0;
                else
                    obj.ind=1;
                end
            end


    end
    end
end

Check Script:

predictedS = EKF(POSITION);
for n = 2:length(POSITION)

[predictedS,S]=predictedS.EKFpredictor(POSITION(ii,:),n);

S1 = S(:,1:3);
 plot3(S1(:,1),S1(:,2),S1(:,3),'g');
hold on
end
hold on
plot3(POSITION(:,1),POSITION(:,2),POSITION(:,3),'b')

POSITION matrix

 -188.1651  187.7193   34.1940
 -185.6452  185.0441   33.8262
 -183.4172  182.3138   33.5098
 -181.4431  179.5418   33.2382
 -179.6895  176.7406   33.0055
 -178.1260  173.9217   32.8063
 -176.7259  171.0961   32.6359
 -175.4649  168.2737   32.4900
 -174.3218  165.4639   32.3650
 -173.2774  162.6754   32.2573
 -172.3147  159.9165   32.1640
 -171.4185  157.1948   32.0825
 -170.5753  154.5171   32.0103
 -169.7732  151.8902   31.9453
 -169.0016  149.3201   31.8858
 -168.2509  146.8122   31.8299
 -167.5129  144.3717   31.7762
 -166.7802  142.0032   31.7235
 -166.0462  139.7109   31.6706
 -165.3053  137.4984   31.6164
 -164.5524  135.3690   31.5602
 -163.7832  133.3256   31.5010
 -162.9939  131.3705   31.4383
 -162.1811  129.5057   31.3715
 -161.3420  127.7328   31.3000
 -160.4744  126.0528   31.2235
 -159.5762  124.4667   31.1416
 -158.6458  122.9747   31.0540
 -157.6819  121.5767   30.9606
 -156.6837  120.2724   30.8610
 -155.6503  119.0611   30.7553
 -154.5815  117.9417   30.6433
 -153.4770  116.9126   30.5250
 -152.3370  115.9722   30.4004
 -151.1617  115.1184   30.2696
 -149.9517  114.3489   30.1327
 -148.7077  113.6611   29.9898
 -147.4306  113.0521   29.8410
 -146.1215  112.5188   29.6866
 -144.7815  112.0579   29.5267
 -143.4120  111.6659   29.3616
 -142.0146  111.3392   29.1915
 -140.5910  111.0738   29.0168
 -139.1428  110.8657   28.8377
 -137.6720  110.7110   28.6545
 -136.1806  110.6053   28.4677
 -134.6706  110.5443   28.2776
 -133.1443  110.5237   28.0845
 -131.6037  110.5391   27.8888
 -130.0513  110.5860   27.6910
 -128.4893  110.6600   27.4914
 -126.9202  110.7566   27.2904
 -125.3463  110.8715   27.0885
 -123.7701  111.0001   26.8860
 -122.1940  111.1383   26.6834
 -120.6205  111.2817   26.4812
 -119.0519  111.4261   26.2796
 -117.4908  111.5676   26.0791
 -115.9394  111.7022   25.8802
 -114.4001  111.8260   25.6832
 -112.8751  111.9353   25.4884
 -111.3667  112.0267   25.2963
 -109.8770  112.0967   25.1072
 -108.4081  112.1421   24.9215
 -106.9620  112.1598   24.7395
 -105.5405  112.1472   24.5614
 -104.1455  112.1014   24.3876
 -102.7785  112.0201   24.2183
 -101.4412  111.9009   24.0539
 -100.1350  111.7419   23.8944
  -98.8612  111.5412   23.7401
  -97.6210  111.2973   23.5912
  -96.4154  111.0086   23.4478
  -95.2454  110.6741   23.3101
  -94.1117  110.2928   23.1782
  -93.0149  109.8639   23.0520
  -91.9556  109.3870   22.9318
  -90.9340  108.8617   22.8174
  -89.9503  108.2879   22.7089
  -89.0047  107.6658   22.6063
  -88.0969  106.9958   22.5095
  -87.2269  106.2782   22.4184
  -86.3941  105.5140   22.3329
  -85.5981  104.7040   22.2529
  -84.8382  103.8493   22.1782
  -84.1138  102.9512   22.1087
  -83.4238  102.0112   22.0441
  -82.7673  101.0310   21.9842
  -82.1431  100.0122   21.9288
  -81.5501   98.9569   21.8777
  -80.9868   97.8670   21.8305
  -80.4519   96.7450   21.7870
  -79.9439   95.5929   21.7468
  -79.4611   94.4133   21.7096
  -79.0018   93.2088   21.6751
  -78.5645   91.9818   21.6430
  -78.1471   90.7352   21.6129
  -77.7481   89.4716   21.5844
  -77.3653   88.1940   21.5572
  -76.9970   86.9052   21.5309
  -76.6412   85.6080   21.5051
  -76.2959   84.3054   21.4794
  -75.9593   83.0003   21.4535
  -75.6292   81.6957   21.4271
  -75.3038   80.3945   21.3996
  -74.9811   79.0995   21.3708
  -74.6593   77.8137   21.3403
  -74.3363   76.5399   21.3077
  -74.0104   75.2808   21.2727
  -73.6798   74.0391   21.2349
  -73.3426   72.8175   21.1941
  -72.9972   71.6185   21.1499
  -72.6420   70.4446   21.1019
  -72.2754   69.2981   21.0500
  -71.8959   68.1812   20.9939
  -71.5020   67.0962   20.9332
  -71.0924   66.0449   20.8678
  -70.6658   65.0292   20.7973
  -70.2212   64.0509   20.7217
  -69.7573   63.1115   20.6407
  -69.2733   62.2126   20.5542
  -68.7682   61.3552   20.4620
  -68.2413   60.5406   20.3639
  -67.6918   59.7697   20.2599
  -67.1192   59.0432   20.1499
  -66.5231   58.3618   20.0337
  -65.9029   57.7260   19.9114
  -65.2584   57.1358   19.7830
  -64.5895   56.5916   19.6483
  -63.8960   56.0931   19.5074
  -63.1780   55.6401   19.3604
  -62.4356   55.2321   19.2072
  -61.6689   54.8686   19.0480
  -60.8783   54.5488   18.8827
  -60.0641   54.2717   18.7117
  -59.2268   54.0363   18.5348
  -58.3669   53.8412   18.3523
  -57.4850   53.6850   18.1642
  -56.5818   53.5663   17.9709
  -55.6581   53.4832   17.7723
  -54.7146   53.4339   17.5688
  -53.7523   53.4165   17.3605
  -52.7720   53.4289   17.1476
  -51.7747   53.4689   16.9303
  -50.7615   53.5341   16.7088
  -49.7333   53.6222   16.4834
  -48.6913   53.7306   16.2543
  -47.6366   53.8569   16.0218
  -46.5702   53.9982   15.7861
  -45.4934   54.1520   15.5474
  -44.4072   54.3156   15.3060
  -43.3128   54.4860   15.0621
  -42.2113   54.6605   14.8161
  -41.1040   54.8363   14.5681
  -39.9919   55.0105   14.3185
  -38.8761   55.1803   14.0674
  -37.7577   55.3428   13.8151
  -36.6378   55.4953   13.5618
  -35.5173   55.6350   13.3078
  -34.3974   55.7592   13.0533
  -33.2788   55.8652   12.7986
  -32.1626   55.9503   12.5437
  -31.0496   56.0122   12.2891
  -29.9405   56.0482   12.0347
  -28.8361   56.0562   11.7810
  -27.7371   56.0337   11.5279
  -26.6441   55.9788   11.2757
  -25.5576   55.8893   11.0245
  -24.4782   55.7633   10.7744
  -23.4063   55.5991   10.5257
  -22.3421   55.3950   10.2784
  -21.2860   55.1496   10.0326
  -20.2382   54.8615    9.7885
  -19.1987   54.5295    9.5460
  -18.1677   54.1526    9.3053
  -17.1452   53.7299    9.0664
  -16.1310   53.2606    8.8294
  -15.1250   52.7443    8.5942
  -14.1269   52.1806    8.3610
  -13.1365   51.5691    8.1297
  -12.1534   50.9100    7.9002
  -11.1772   50.2033    7.6727
  -10.2073   49.4492    7.4470
   -9.2433   48.6483    7.2231
   -8.2846   47.8012    7.0010
   -7.3304   46.9086    6.7805
   -6.3802   45.9715    6.5617
   -5.4332   44.9909    6.3445
   -4.4886   43.9681    6.1287
   -3.5456   42.9044    5.9143
   -2.6035   41.8014    5.7012
   -1.6614   40.6607    5.4893
   -0.7184   39.4840    5.2785
    0.2263   38.2733    5.0686
    1.1735   37.0305    4.8597
    2.1241   35.7577    4.6515
    3.0788   34.4572    4.4439
    4.0386   33.1311    4.2370
    5.0041   31.7819    4.0305
    5.9760   30.4120    3.8244
    6.9551   29.0239    3.6185
    7.9419   27.6201    3.4129
    8.9370   26.2033    3.2074
    9.9409   24.7760    3.0020
   10.9540   23.3409    2.7966
   11.9766   21.9007    2.5911
   13.0089   20.4580    2.3856
   14.0511   19.0157    2.1800
   15.1030   17.5763    1.9744
   16.1647   16.1425    1.7687
   17.2359   14.7171    1.5630
   18.3161   13.3025    1.3573
   19.4050   11.9015    1.1518
   20.5018   10.5166    0.9464
   21.6057    9.1502    0.7414
   22.7157    7.8049    0.5369
   23.8307    6.4831    0.3330
   24.9495    5.1870    0.1299
   26.0705    3.9191   -0.0722
   27.1922    2.6814   -0.2731
   28.3126    1.4762   -0.4724
   29.4298    0.3055   -0.6699
   30.5417   -0.8288   -0.8653
   31.6458   -1.9246   -1.0583
   32.7397   -2.9803   -1.2485
   33.8206   -3.9940   -1.4355
   34.8857   -4.9642   -1.6188
   35.9320   -5.8892   -1.7982
   36.9561   -6.7676   -1.9730
   37.9548   -7.5979   -2.1429
   38.9245   -8.3790   -2.3072
   39.8615   -9.1096   -2.4656
   40.7621   -9.7885   -2.6174
   41.6224  -10.4148   -2.7620
   42.4382  -10.9875   -2.8990
   43.2055  -11.5058   -3.0276
   43.9201  -11.9690   -3.1473
   44.5777  -12.3765   -3.2574
   45.1739  -12.7276   -3.3574
   45.7044  -13.0220   -3.4465
   46.1648  -13.2593   -3.5241
   46.5509  -13.4394   -3.5896
   46.8584  -13.5622   -3.6424
   47.0830  -13.6277   -3.6817
   47.2206  -13.6362   -3.7069
   47.2673  -13.5878   -3.7175
   47.2193  -13.4831   -3.7128
   47.0731  -13.3228   -3.6923
   46.8252  -13.1076   -3.6554
   46.4728  -12.8385   -3.6015
   46.0132  -12.5166   -3.5302
   45.4440  -12.1435   -3.4412
   44.7635  -11.7207   -3.3339
   43.9705  -11.2500   -3.2081
   43.0642  -10.7335   -3.0636
   42.0447  -10.1736   -2.9001
   40.9125   -9.5729   -2.7177
   39.6694   -8.9344   -2.5163
   38.3176   -8.2612   -2.2962
   36.8606   -7.5569   -2.0575
   35.3031   -6.8254   -1.8007
   33.6507   -6.0708   -1.5265
   31.9107   -5.2976   -1.2355
   30.0915   -4.5108   -0.9289
   28.2036   -3.7154   -0.6076
   26.2588   -2.9170   -0.2733
   24.2713   -2.1214    0.0723
   22.2570   -1.3346    0.4272
   20.2347   -0.5630    0.7891
   18.2252    0.1868    1.1551
   16.2524    0.9082    1.5222
   14.3433    1.5943    1.8866
   12.5280    2.2383    2.2443
   10.8405    2.8333    2.5905
    9.3185    3.3728    2.9199
    8.0041    3.8501    3.2266
    6.9442    4.2595    3.5039
    6.1905    4.5954    3.7442
    5.8005    4.8530    3.9391

Velocity vector :

  747.0176 -736.8417 -110.3954
  660.0126 -754.1758  -95.0541
  584.1147 -767.6202  -81.6712
  518.1547 -777.4587  -70.0407
  461.0804 -783.9474  -59.9769
  411.9453 -787.3191  -51.3131
  369.8994 -787.7867  -43.8993
  334.1793 -785.5465  -37.6009
  304.1001 -780.7806  -32.2971
  279.0476 -773.6596  -27.8797
  258.4712 -764.3446  -24.2515
  241.8777 -752.9889  -21.3256
  228.8250 -739.7391  -19.0241
  218.9174 -724.7368  -17.2771
  211.8001 -708.1194  -16.0221
  207.1551 -690.0205  -15.2031
  204.6971 -670.5712  -14.7697
  204.1703 -649.8998  -14.6768
  205.3444 -628.1329  -14.8839
  208.0123 -605.3948  -15.3543
  211.9872 -581.8082  -16.0553
  217.1002 -557.4940  -16.9569
  223.1984 -532.5713  -18.0323
  230.1428 -507.1572  -19.2570
  237.8068 -481.3667  -20.6086
  246.0746 -455.3126  -22.0667
  254.8402 -429.1053  -23.6127
  264.0056 -402.8525  -25.2293
  273.4808 -376.6587  -26.9007
  283.1821 -350.6256  -28.6122
  293.0316 -324.8513  -30.3500
  302.9568 -299.4301  -32.1014
  312.8900 -274.4525  -33.8545
  322.7675 -250.0051  -35.5982
  332.5296 -226.1698  -37.3220
  342.1204 -203.0241  -39.0161
  351.4872 -180.6409  -40.6713
  360.5803 -159.0881  -42.2789
  369.3532 -138.4289  -43.8309
  377.7625 -118.7209  -45.3196
  385.7672 -100.0170  -46.7381
  393.3294  -82.3645  -48.0796
  400.4140  -65.8056  -49.3383
  406.9884  -50.3772  -50.5085
  413.0230  -36.1107  -51.5851
  418.4909  -23.0322  -52.5637
  423.3679  -11.1627  -53.4400
  427.6328   -0.5178  -54.2107
  431.2672    8.8919  -54.8725
  434.2555   17.0610  -55.4232
  436.5850   23.9889  -55.8605
  438.2461   29.6799  -56.1832
  439.2320   34.1430  -56.3902
  439.5386   37.3917  -56.4811
  439.1652   39.4439  -56.4561
  438.1136   40.3216  -56.3157
  436.3887   40.0510  -56.0612
  433.9980   38.6619  -55.6941
  430.9519   36.1879  -55.2167
  427.2637   32.6660  -54.6315
  422.9491   28.1362  -53.9417
  418.0263   22.6418  -53.1508
  412.5162   16.2286  -52.2629
  406.4418    8.9450  -51.2822
  399.8286    0.8419  -50.2137
  392.7040   -8.0280  -49.0624
  385.0975  -17.6100  -47.8340
  377.0402  -27.8478  -46.5342
  368.5651  -38.6837  -45.1692
  359.7067  -50.0589  -43.7454
  350.5007  -61.9136  -42.2693
  340.9841  -74.1874  -40.7478
  331.1949  -86.8191  -39.1879
  321.1718  -99.7478  -37.5966
  310.9543 -112.9119  -35.9812
  300.5823 -126.2505  -34.3490
  290.0961 -139.7028  -32.7073
  279.5359 -153.2086  -31.0634
  268.9420 -166.7085  -29.4246
  258.3544 -180.1440  -27.7981
  247.8127 -193.4578  -26.1911
  237.3559 -206.5940  -24.6106
  227.0223 -219.4979  -23.0633
  216.8492 -232.1168  -21.5561
  206.8730 -244.3996  -20.0953
  197.1286 -256.2972  -18.6872
  187.6500 -267.7626  -17.3378
  178.4695 -278.7510  -16.0527
  169.6176 -289.2200  -14.8372
  161.1234 -299.1297  -13.6966
  153.0142 -308.4427  -12.6353
  145.3153 -317.1243  -11.6579
  138.0499 -325.1425  -10.7682
  131.2395 -332.4684   -9.9699
  124.9031 -339.0756   -9.2661
  119.0578 -344.9411   -8.6595
  113.7184 -350.0445   -8.1526
  108.8976 -354.3688   -7.7473
  104.6055 -357.9000   -7.4450
  100.8503 -360.6271   -7.2470
   97.6377 -362.5424   -7.1537
   94.9714 -363.6413   -7.1656
   92.8526 -363.9221   -7.2824
   91.2804 -363.3865   -7.5035
   90.2517 -362.0391   -7.8279
   89.7614 -359.8877   -8.2544
   89.8022 -356.9429   -8.7810
   90.3649 -353.2184   -9.4058
   91.4382 -348.7306  -10.1262
   93.0092 -343.4990  -10.9393
   95.0630 -337.5454  -11.8421
   97.5833 -330.8946  -12.8311
  100.5519 -323.5737  -13.9025
  103.9493 -315.6124  -15.0523
  107.7547 -307.0426  -16.2764
  111.9460 -297.8982  -17.5701
  116.4998 -288.2156  -18.9288
  121.3918 -278.0326  -20.3477
  126.5969 -267.3893  -21.8216
  132.0890 -256.3269  -23.3455
  137.8415 -244.8885  -24.9139
  143.8271 -233.1183  -26.5216
  150.0183 -221.0617  -28.1631
  156.3871 -208.7650  -29.8329
  162.9055 -196.2755  -31.5255
  169.5453 -183.6411  -33.2352
  176.2784 -170.9099  -34.9567
  183.0769 -158.1307  -36.6844
  189.9133 -145.3521  -38.4130
  196.7603 -132.6229  -40.1371
  203.5912 -119.9914  -41.8515
  210.3801 -107.5057  -43.5511
  217.1015  -95.2132  -45.2309
  223.7308  -83.1607  -46.8862
  230.2446  -71.3937  -48.5123
  236.6200  -59.9571  -50.1048
  242.8356  -48.8942  -51.6594
  248.8708  -38.2469  -53.1722
  254.7064  -28.0557  -54.6392
  260.3244  -18.3593  -56.0570
  265.7082   -9.1946  -57.4223
  270.8424   -0.5963  -58.7320
  275.7132    7.4028  -59.9834
  280.3082   14.7722  -61.1739
  284.6165   21.4837  -62.3012
  288.6286   27.5117  -63.3636
  292.3367   32.8331  -64.3593
  295.7345   37.4273  -65.2869
  298.8171   41.2764  -66.1453
  301.5814   44.3656  -66.9338
  304.0257   46.6825  -67.6519
  306.1498   48.2177  -68.2993
  307.9551   48.9650  -68.8761
  309.4445   48.9206  -69.3826
  310.6223   48.0841  -69.8195
  311.4943   46.4578  -70.1875
  312.0676   44.0468  -70.4879
  312.3508   40.8594  -70.7220
  312.3537   36.9066  -70.8914
  312.0872   32.2023  -70.9979
  311.5635   26.7629  -71.0437
  310.7962   20.6079  -71.0309
  309.7994   13.7592  -70.9620
  308.5885    6.2411  -70.8398
  307.1798   -1.9194  -70.6668
  305.5904  -10.6931  -70.4462
  303.8381  -20.0485  -70.1810
  301.9412  -29.9521  -69.8744
  299.9188  -40.3685  -69.5297
  297.7903  -51.2602  -69.1504
  295.5756  -62.5883  -68.7399
  293.2947  -74.3123  -68.3017
  290.9680  -86.3903  -67.8395
  288.6157  -98.7791  -67.3568
  286.2582 -111.4346  -66.8572
  283.9155 -124.3118  -66.3442
  281.6077 -137.3651  -65.8216
  279.3543 -150.5482  -65.2926
  277.1744 -163.8145  -64.7609
  275.0865 -177.1173  -64.2297
  273.1086 -190.4099  -63.7022
  271.2577 -203.6459  -63.1817
  269.5502 -216.7790  -62.6711
  268.0011 -229.7636  -62.1731
  266.6248 -242.5548  -61.6906
  265.4342 -255.1087  -61.2260
  264.4410 -267.3821  -60.7815
  263.6555 -279.3332  -60.3592
  263.0866 -290.9217  -59.9609
  262.7417 -302.1084  -59.5883
  262.6263 -312.8558  -59.2427
  262.7443 -323.1285  -58.9250
  263.0980 -332.8923  -58.6362
  263.6875 -342.1156  -58.3766
  264.5112 -350.7683  -58.1464
  265.5654 -358.8228  -57.9456
  266.8443 -366.2534  -57.7737
  268.3400 -373.0370  -57.6298
  270.0425 -379.1524  -57.5129
  271.9396 -384.5812  -57.4215
  274.0168 -389.3070  -57.3539
  276.2576 -393.3159  -57.3078
  278.6429 -396.5966  -57.2810
  281.1516 -399.1398  -57.2704
  283.7604 -400.9389  -57.2730
  286.4436 -401.9895  -57.2853
  289.1734 -402.2894  -57.3035
  291.9198 -401.8388  -57.3233
  294.6508 -400.6400  -57.3405
  297.3323 -398.6973  -57.3501
  299.9281 -396.0171  -57.3472
  302.4002 -392.6077  -57.3264
  304.7089 -388.4793  -57.2821
  306.8127 -383.6438  -57.2084
  308.6685 -378.1145  -57.0992
  310.2318 -371.9065  -56.9483
  311.4570 -365.0361  -56.7490
  312.2969 -357.5209  -56.4949
  312.7038 -349.3797  -56.1789
  312.6291 -340.6323  -55.7943
  312.0233 -331.2993  -55.3339
  310.8370 -321.4023  -54.7907
  309.0203 -310.9634  -54.1575
  306.5237 -300.0055  -53.4272
  303.2976 -288.5519  -52.5927
  299.2935 -276.6264  -51.6469
  294.4634 -264.2530  -50.5830
  288.7607 -251.4562  -49.3941
  282.1402 -238.2607  -48.0735
  274.5586 -224.6914  -46.6150
  265.9744 -210.7734  -45.0123
  256.3492 -196.5323  -43.2595
  245.6468 -181.9936  -41.3513
  233.8348 -167.1834  -39.2826
  220.8840 -152.1281  -37.0485
  206.7696 -136.8545  -34.6451
  191.4712 -121.3901  -32.0686
  174.9732 -105.7631  -29.3162
  157.2656  -90.0024  -26.3854
  138.3444  -74.1381  -23.2746
  118.2119  -58.2015  -19.9830
   96.8777  -42.2254  -16.5107
   74.3588  -26.2440  -12.8587
   50.6806  -10.2936   -9.0288
   25.8775    5.5875   -5.0244
   -0.0066   21.3585   -0.8496
  -26.9173   36.9762    3.4899
  -54.7889   52.3945    7.9870
  -83.5435   67.5639   12.6332
 -113.0904   82.4321   17.4183
 -143.3245   96.9430   22.3299
 -174.1256  111.0367   27.3542
 -205.3573  124.6499   32.4748
 -236.8650  137.7150   37.6730
 -268.4755  150.1608   42.9277
 -299.9947  161.9120   48.2145
 -331.2061  172.8897   53.5064
 -361.8693  183.0114   58.7727
 -391.7174  192.1914   63.9788
 -420.4557  200.3412   69.0863
 -447.7585  207.3702   74.0521
 -473.2670  213.1862   78.8280
 -496.5864  217.6966   83.3605
 -517.2832  220.8091   87.5901
 -534.8813  222.4334   91.4506
 -548.8589  222.4823   94.8685
 -558.6446  220.8739   97.7624
 -563.6130  217.5331  100.0421
 -563.0808  212.3945  101.6081
 -556.3010  205.4042  102.3500
 -542.4587  196.5236  102.1461
 -520.6649  185.7317  100.8622
 -489.9507  173.0295   98.3504
 -449.2610  158.4434   94.4478
 -397.4475  142.0298   88.9752
 -333.2619  123.8798   81.7359
 -255.3477  104.1241   72.5139
 -162.2325   82.9386   61.0724
  -52.3195   60.5506   47.1521

EDIT 1:

I'v implemented LKF as per your suggestion, however i'm still not tracking any point. Please see my implementation.

classdef EKF <handle
    properties (Access=private)
        H
        K
        Z
        Q
        M
        ind
        A
        X
        Xh
        P
        a
        b
    end
    methods
        function obj = EKF(position)
            obj.Q =[0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0.01 0 0;
                0 0 0 0 0.01 0;
                0 0 0 0 0 0.01];% Covarience matrix of process noise
            obj.M=[0.001 0 0;
                0 0.001 0;
                0 0 0.001]; % Covarience matrix of measurment noise
            obj.A=[1 0 0 0.1 0 0;
                0 1 0 0 0.1 0;
                0 0 1 0 0 0.1;
                0 0 0 1 0 0;
                0 0 0 0 1 0;
                0 0 0 0 0 1];  % System Dynamics

            obj.X        = zeros(6,1);
            obj.X(1:3,1) = position(1,:);
            obj.P = [0.1 0 0 0 0 0;
                0 0.1 0 0 0 0;
                0 0 0.1 0 0 0;
                0 0 0 0.1 0 0;
                0 0 0 0 0.1 0;
                0 0 0 0 0 0.1];

        end

        function [obj,predictedS]=EKFpredictor(obj,p)
            %% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
            % State process % w generating process noise
            obj.X = obj.A*obj.X+ ...
                [0;0;0;sqrt(obj.Q(4,4))*randn(1);sqrt(obj.Q(5,5))*randn(1); ...
                sqrt(obj.Q(6,6))*randn(1)];
            %         predictedX=obj.Xp;
            predictedS=obj.X';
            %% PREDICTION OF NEXT STATE

            obj.P=obj.A*obj.P*obj.A'+obj.Q;
            %% CORRECTION EQUTIONS
            % Jacobian
            obj.Z = p';
            obj.H = zeros(3,6);
            obj.H(1,1) = 1;
            obj.H(2,2) = 1;
            obj.H(3,3) = 1;

            % Kalman Gain
            S = obj.H*obj.P*obj.H' + obj.M;
            obj.K = obj.P*obj.H'*inv(S);
            % INNOVATION
            Y     = obj.Z - obj.H*obj.X;
            obj.X = obj.X + obj.K*Y;
            obj.P = (eye(6)-obj.K*obj.H)*obj.P; % alternatives exist for this calculation


        end
    end
end

Checking the LKF:

 predictedS = EKF(POSITION);
for n = 2:length(POSITION)

[predictedS,S]=predictedS.EKFpredictor(POSITION(ii,:));

S1 = S(:,1:3);
 plot3(S1(:,1),S1(:,2),S1(:,3),'g');
hold on
end

Upvotes: 1

Views: 1550

Answers (1)

Geoff
Geoff

Reputation: 1603

Out of curiosity, why are you using an Extended Kalman Filter (EKF)? Since you are tracking an object in 3D space, with each position (measurement or observation) input given by an (x,y,z) triple), and the (output) state vector (X) is a 3D position (with velocity components), why not just use the simpler Linear Kalman Filter (LKF)? That way you avoid the conversions from the (x,y,z) coordinate space to a range and bearing, you avoid the first-order derivatives for the Jacobian, etc.

Since your goal is to estimate the position, I am going to suggest that you use the LKF, and I will describe it below while stepping through your code.

INITIALIZATION

Your POSITION matrix is 279x3 (same for VELOCITY), so that means we have 279 observations that will be used to correct (or update) the object. For the initialization, we need just one position (I'm going to leave out the velocity for now) so rather than

predictedS = EKF(POSITION);

we can do

predictedS = EKF(POSITION(1,:));

The constructor for your class instantiates some matrices (Q,A,M) with some default values and then initializes the state vector and covariance matrix, X and P respectively:

obj.X(:,1)=[position(1,:) position(2,:)];
obj.P(:,:,1)=[0.1 0 0 0 0 0;
            0 0.1 0 0 0 0;
            0 0 0.1 0 0 0;
            0 0 0 0.1 0 0;
            0 0 0 0 0.1 0;
            0 0 0 0 0 0.1];

I'm ignoring the initializations of Z and Xh. For simplicity, I am also going to ignore the input parameter n and so not keep track of the historical information.

In your initialization of X, it becomes a 6x1 vector where the first three elements correspond to the first observation - which makes sense since these are the (x,y,z) co-ordinates - and the last three elements are set to the second observation (this was when you were passing in all 279 observation). This is incorrect, as the velocities (vx,vy,vz) of X are being given incorrect values - positions rather than velocities. If you don't know the initial velocities of the object, then the Kalman Filter will estimate them over time. So we can simply replace the above state initialization with 

obj.X        = zeros(6,1);
obj.X(1:3,1) = position(1,:);

A simpler initialization of the covariance initialization becomes 

obj.P = [0.1 0 0 0 0 0;
         0 0.1 0 0 0 0;
         0 0 0.1 0 0 0;
         0 0 0 0.1 0 0;
         0 0 0 0 0.1 0;
         0 0 0 0 0 0.1];

You've assigned the same error uncertainty (variance) for each component (x,y,z,vx,vy,vz). This may be unrealistic as typically position uncertainties have larger variances. Are you guessing at these values or are they based on some other knowledge?

As an aside, what are the units of the elements in the state vector? Metres and metres per second, or something similar? I am asking in part because your velocities (in VELOCITIES) are large compared to the changes in your positions and in part because the initialization defines the transition matrix as

obj.A=[1 0 0 0.1 0 0;
       0 1 0 0 0.1 0;
       0 0 1 0 0 0.1;
       0 0 0 1 0 0;
       0 0 0 0 1 0;
       0 0 0 0 0 1];

The 0.1 refers to the unit of time between each update. What is that in this case? One tenth of a second? If so that doesn't seem to jive with the velocities and the position changes (unless I missed something).

PREDICTION

The code calls the prediction (and subsequently correction) as follows

[predictedS,S]=predictedS.EKFpredictor(POSITION(ii,:),n);

The ii is undefined, and I'm unclear why the n is being passed in (unless you want to keep a historical record of all predictions and updates). I'll ignore that for now and change this to

[predictedS,S]=tracker.EKFpredictor(POSITION(n,:));

where the nth position (starting from n==2) is passed in to the predictor.

Your prediction code (for the state vector) looks something like

obj.X(:,n-1)=[obj.X(1:3,n-1)' p]'; 
obj.X(:,n)=obj.A*obj.X(:,n-1)+ ...
    [0;0;0;sqrt(obj.Q(4,4))*randn(1);sqrt(obj.Q(5,5))*randn(1); ...
     sqrt(obj.Q(6,6))*randn(1)];

It's not all that clear to me why the first line takes the position elements from X and concatenates it with the new position p. This seems incorrect as there is twice the position information in the state vector, and the velocity info is gone. I think that the first line can (should?) be ignored and replaced with just

obj.X = obj.A*obj.X+ …
    [0;0;0;sqrt(obj.Q(4,4))*randn(1);sqrt(obj.Q(5,5))*randn(1); ...
     sqrt(obj.Q(6,6))*randn(1)];
        predictedX=obj.Xp;

with the predicted state vector set to the above (I'm assuming that is what predictedS is referring to)

predictedS=obj.X(:,n)';

There is no need to hold on to the Xh at this point.

The covariance can be predicted as you have shown (I've just removed the n)

obj.P=obj.A*obj.P*obj.A'+obj.Q;

CORRECTION

With the LKF, the correction is much simpler. The observation or measurement matrix Z is simply

obj.Z = p';

The Jacobian is just

obj.H = zeros(3,6);
obj.H(1,1) = 1;
obj.H(2,2) = 1;
obj.H(3,3) = 1;

The Kalman Gain is

S = obj.H*obj.P*obj.H' + obj.M;
obj.K = obj.P*obj.H'*inv(S);

Note that in the above the use of M which has your measurement/observation uncertainties which is defaulted to

obj.M=[0.001 0 0;
       0 0.001 0;
       0 0 0.001];

Again the units are important, and given the incredibly small values, this means that the measurement/observation/new position will be weighted heavier when used to correct the track - and so the new position will greatly influence the corrected state vector.

In your code, you did something similar when calculating S

(obj.M+obj.H(:,:,n-1)*obj.P(:,:,n)*obj.H(:,:,n-1)')^(-1)

Use of ^(-1) is not appropriate for the inverse of a matrix. inv(A) is one alternative that is probably sufficient in this case.

The innovation and correction follows

Y     = obj.Z - obj.H*obj.X;
obj.X = obj.X + obj.K*Y;
obj.P = (eye(6)-obj.K*obj.H)*obj.P; % alternatives exist for this calculation

Consider using the LKF - it should provide a better estimate of position since you can ignore the conversions to range and bearing and back again. Also re-check the calculations for the velocities. Do they make sense given the changes in position and the transition times defined in your A matrix.

Upvotes: 4

Related Questions