我试图在三维空间中跟踪一个物体，其中我是一个物体位置和方向速度。为此，我在 Matlab 中编写了一个类，然而，我的跟踪算法EKF的方程式/算法工作正常，因为每个当前和之前的状态都被预测得很好，但是，我想输入一个轨迹Nx3点，我得到了这个错误。
但是我也有方向速度的信息，即位置矢量的导数。
所有我在位置估算/预测仅输入位置或速度方面感到困惑，如obj.X(:,1) = [6x1]和obj.Xh(:,1) = [6x1]，这意味着[x,y,z,vx,vy,vz]？
如果是这样，我如何 INPUT 来检查它的估计，如果不是，我怎么能估计POSITION，因为我的目标只是估计位置。
我的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
检查脚本：
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矩阵
-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
速度矢量：
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
编辑1：
我根据您的建议实施 LKF ，但我仍然没有跟踪任何一点。 请参阅我的实施。
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
检查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
答案 0 :(得分：4)
出于好奇，你为什么要使用扩展卡尔曼滤波器（EKF）？因为您正在跟踪3D空间中的对象，每个位置（测量或观察）输入由（x，y，z）三元组给出，并且（输出）状态向量（X）是3D位置（具有速度分量） ），为什么不使用更简单的线性卡尔曼滤波器（LKF）？这样你就可以避免从（x，y，z）坐标空间到范围和方位的转换，避免使用雅可比行列式的一阶导数等。
由于你的目标是估计位置，我建议你使用LKF，我会在逐步完成你的代码时对它进行描述。
<强> INITIALIZATION
您的POSITION矩阵是279x3（VELOCITY相同），这意味着我们有279个观察值将用于纠正（或更新）对象。对于初始化，我们只需要一个位置（我现在要省略速度），而不是
predictedS = EKF(POSITION);
我们可以做到
predictedS = EKF(POSITION(1,:));
您的类的构造函数使用一些默认值实例化一些矩阵（Q，A，M），然后初始化状态向量和协方差矩阵X和分别为P：
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];
我忽略了Z和Xh的初始化。为简单起见，我也将忽略输入参数n，因此不会跟踪历史信息。
在X的初始化中，它变为6x1向量，其中前三个元素对应于第一个观察 - 这是有道理的，因为它们是（x，y，z）坐标 - 而最后一个三个元素设置为第二个观察（这是当你通过所有279观察时）。这是不正确的，因为X的速度（vx，vy，vz）被赋予了不正确的值 - 位置而不是速度。如果您不知道物体的初始速度，那么卡尔曼滤波器将随时间估计它们。所以我们可以简单地用
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];
您为每个组件（x，y，z，vx，vy，vz）分配了相同的错误不确定性（方差）。这可能是不现实的，因为通常位置不确定性具有较大的差异。您是在猜测这些价值观还是基于其他一些知识？
顺便说一下，状态向量中元素的单位是多少？每秒米和米，或类似的东西？我问的部分是因为你的速度（VELOCITIES）与你的位置变化相比很大，部分是因为初始化将转换矩阵定义为
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];
0.1表示每次更新之间的时间单位。在这种情况下是什么？十分之一秒？如果是这样的话似乎没有速度和位置的变化（除非我错过了什么）。
<强>预测
代码调用预测（以及随后的修正）如下
[predictedS,S]=predictedS.EKFpredictor(POSITION(ii,:),n);
ii未定义，我不清楚为什么传递n（除非您想保留所有预测和更新的历史记录）。我现在忽略它并将其改为
[predictedS,S]=tracker.EKFpredictor(POSITION(n,:));
其中第n个位置（从n==2开始）传递给预测变量。
您的预测代码（状态向量）类似于
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)];
我并不清楚为什么第一行从X获取位置元素并将其与新位置p连接起来。这似乎是不正确的，因为状态向量中的位置信息是两倍，并且速度信息消失了。我认为第一行可以（应该？）被忽略并替换为
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指的是我的假设）
predictedS=obj.X(:,n)';
此时无需坚持Xh。
协方差可以预测，如您所示（我刚删除了n）
obj.P=obj.A*obj.P*obj.A'+obj.Q;
<强> CORRECTION
使用LKF，校正更简单。观察或测量矩阵Z只是
obj.Z = p';
Jacobian只是
obj.H = zeros(3,6);
obj.H(1,1) = 1;
obj.H(2,2) = 1;
obj.H(3,3) = 1;
卡尔曼增益
S = obj.H*obj.P*obj.H' + obj.M;
obj.K = obj.P*obj.H'*inv(S);
请注意，在上文中使用的M具有您的测量/观察不确定性，默认为
obj.M=[0.001 0 0;
0 0.001 0;
0 0 0.001];
同样单位很重要，并且给定非常小的值，这意味着当用于校正轨道时，测量/观察/新位置将被加权更重 - 因此新位置将极大地影响校正的状态向量。
在您的代码中，您在计算S
(obj.M+obj.H(:,:,n-1)*obj.P(:,:,n)*obj.H(:,:,n-1)')^(-1)
使用^(-1)不适合矩阵的逆矩阵。在这种情况下，inv(A)是一种可能已经足够的替代方案。
创新和纠正遵循
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
考虑使用LKF - 它应该提供更好的位置估计，因为您可以忽略转换到范围和方位并再次返回。还要重新检查速度的计算。考虑到A矩阵中定义的位置变化和过渡时间，它们是否有意义。