我正在Matlab中设计一个卡尔曼滤波器，但我想设置初始值。我已经设置了以下矩阵并找到了设置初始化观察的方法。
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
M=[0.001 0 0; 0 0.001 0; 0 0 0.001]; % Covarience matrix of measurment noise
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
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]; %inital value of covarience of estimation error
我想设置初始观察，因为我在Matrix X中有实际初始条件，观察矩阵和其余假设的假设初始化是什么。
编辑1： 我在Matlab中实现了EKF（扩展卡尔曼滤波器），用于对象的3D轨迹的视觉跟踪，但是，我将实际轨迹的位置和速度分别设为in1和in2。在经常与实际轨迹相反的一些点之后，我面临着错误的预测。我认为，这可能是一些初步的假设问题，因为我多次检查方程但无法找到错误。如何解决我的初步假设或避免任何其他猜测？给出样品位置和速度。欢迎任何改进我的方法的建议。 我的代码：
function EKF(in1,in2)
ind=0; % indicator function. Used for unwrapping of tan
[H] = [];
[K] = [];
[Z] = [];
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
M=[0.001 0 0; 0 0.001 0; 0 0 0.001]; % Covarience matrix of measurment noise
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
in = cat(2,in1,in2);
X(:,1)=in(1,:)'; % Actual initial conditions
Z(:,1)=X(1:3,:);% initial observation
Xh(:,1)=X(:,1);%Assumed initial conditions
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
% plots
subplot(3,3,1)
xlabel('time')
ylabel('X')
title('X possition')
hold on
subplot(3,3,4)
xlabel('time')
ylabel('Y')
title('Y possition')
hold on
subplot(3,3,7)
xlabel('time')
ylabel('Z')
title('Z possition')
hold on
subplot(2,2,2)
xlabel('time')
title('Minimum MSE')
hold on
subplot(2,2,4)
plot3(0,0,0)
title('3-D trajectory ')
xlabel('X')
ylabel('Y')
zlabel('Z')
hold on
for n=1:100
%% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
X(:,n+1)=A*X(:,n)+[0;0;0;sqrt(Q(4,4))*randn(1);sqrt(Q(5,5))*randn(1);sqrt(Q(6,6))*randn(1)]; % State process % w generating process noise
Z(:,n+1)=[sqrt(X(1,n)^2+X(2,n)^2);arctang(X(2,n),X(1,n),ind);X(3,n)]+[sqrt(M(1,1))*randn(1);sqrt(M(1,1))*randn(1);sqrt(M(1,1))*randn(1)]; %generating & observation observation noise
%% prediction of next state
Xh(:,n+1)=A*Xh(:,n);% ESTIMATE
P(:,:,n+1)=A*P(:,:,n)*A'+Q;% PRIORY ERROR COVARIENCE
%% CORRECTION EQUTIONS
H(:,:,n+1)=[Xh(1,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)), Xh(2,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)),0,0,0,0; ...
-Xh(2,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)), Xh(1,n+1)/(sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2)),0,0,0,0; ...
0,0,1,0,0,0]; % Jacobian matrix
K(:,:,n+1)=P(:,:,n+1)*H(:,:,n+1)'*(M+H(:,:,n+1)*P(:,:,n+1)*H(:,:,n+1)')^(-1); % Kalman Gain
Inov=Z(:,n+1)-[sqrt(Xh(1,n+1)^2+Xh(2,n+1)^2);arctang(Xh(2,n+1),Xh(1,n+1),ind);Xh(3,n+1)];% INNOVATION
Xh(:,n+1)=Xh(:,n+1)+ K(:,:,n+1)*Inov; %computes final estimate
P(:,:,n+1)=(eye(6)-K(:,:,n+1)*H(:,:,n+1))*P(:,:,n+1);% %computes covarience of estimation error
%% unwrapping the tan function
theta1=arctang(Xh(1,n+1),Xh(2,n+1),0);
theta=arctang(Xh(1,n),Xh(2,n),0);
if abs(theta1-theta)>=pi
if ind==1
ind=0;
else
ind=1;
end
end
%% Some Plots
subplot(3,3,1)
line([n,n+1],[X(1,n),X(1,n+1)])
hold on
drawnow
subplot(3,3,4)
line([n,n+1],[X(2,n),X(2,n+1)])
hold on
drawnow
subplot(3,3,7)
line([n,n+1],[X(3,n),X(3,n+1)])
hold on
drawnow
subplot(2,2,4)
line([Xh(1,n) Xh(1,n+1)],[Xh(2,n) Xh(2,n+1)],[Xh(3,n) Xh(3,n+1)],'Color','r')
hold on
drawnow
line([X(1,n) X(1,n+1)],[X(2,n) X(2,n+1)],[X(3,n) X(3,n+1)])
hold on
drawnow
subplot(2,2,2)
line([n,n+1],[(X(1,n)-Xh(1,n))^2,(X(1,n+1)-Xh(1,n+1))^2])
hold on
drawnow
line([n,n+1],[(X(2,n)-Xh(2,n))^2,(X(2,n+1)-Xh(2,n+1))^2],'Color','r')
hold on
drawnow
line([n,n+1],[(X(3,n)-Xh(3,n))^2,(X(3,n+1)-Xh(3,n+1))^2],'Color','c')
hold on
drawnow
legend('X-MSE','Y-MSE','Z-MSE')
subplot(3,3,1)
line([n,n+1],[Xh(1,n),Xh(1,n+1)],'Color','r')
hold on
drawnow
subplot(3,3,4)
line([n,n+1],[Xh(2,n),Xh(2,n+1)],'Color','r')
hold on
drawnow
subplot(3,3,7)
line([n,n+1],[Xh(3,n),Xh(3,n+1)],'Color','r')
hold on
drawnow
end
%% arctang
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
end
样本位置矩阵in1：
-191.2442 187.7193 1.0000
-188.3010 184.5880 1.0880
-185.7538 181.3845 1.1760
-183.5429 178.1301 1.2640
-181.6158 174.8440 1.3520
-179.9268 171.5442 1.4400
-178.4357 168.2469 1.5280
-177.1073 164.9674 1.6160
-175.9110 161.7195 1.7040
-174.8199 158.5160 1.7920
-173.8109 155.3688 1.8800
-172.8634 152.2884 1.9680
-171.9599 149.2849 2.0560
-171.0849 146.3669 2.1440
-170.2251 143.5427 2.2320
-169.3691 140.8192 2.3200
-168.5068 138.2028 2.4080
-167.6298 135.6988 2.4960
-166.7307 133.3120 2.5840
-165.8035 131.0459 2.6720
-164.8429 128.9038 2.7600
-163.8447 126.8877 2.8480
-162.8053 124.9991 2.9360
-161.7219 123.2387 3.0240
-160.5925 121.6065 3.1120
-159.4156 120.1018 3.2000
-158.1900 118.7233 3.2879
-156.9155 117.4689 3.3759
-155.5919 116.3360 3.4639
-154.2197 115.3214 3.5518
-152.7997 114.4215 3.6398
-151.3331 113.6319 3.7277
-149.8214 112.9480 3.8156
-148.2665 112.3647 3.9035
-146.6706 111.8764 3.9913
-145.0360 111.4775 4.0791
-143.3655 111.1617 4.1669
-141.6619 110.9226 4.2546
-139.9283 110.7537 4.3422
-138.1681 110.6481 4.4298
-136.3846 110.5991 4.5173
-134.5815 110.5995 4.6047
-132.7624 110.6424 4.6919
-130.9310 110.7206 4.7791
-129.0913 110.8272 4.8661
-127.2472 110.9553 4.9529
-125.4024 111.0978 5.0395
-123.5610 111.2482 5.1260
-121.7268 111.3998 5.2121
-119.9036 111.5463 5.2981
-118.0952 111.6816 5.3837
-116.3052 111.7997 5.4690
-114.5371 111.8949 5.5539
-112.7944 111.9621 5.6384
-111.0803 111.9962 5.7225
-109.3980 111.9924 5.8062
-107.7502 111.9465 5.8893
-106.1398 111.8546 5.9718
-104.5691 111.7130 6.0537
-103.0406 111.5185 6.1350
-101.5561 111.2684 6.2156
-100.1174 110.9602 6.2954
-98.7262 110.5919 6.3744
-97.3835 110.1619 6.4525
-96.0906 109.6691 6.5297
-94.8480 109.1126 6.6059
-93.6563 108.4921 6.6810
-92.5156 107.8075 6.7551
-91.4259 107.0592 6.8279
-90.3868 106.2479 6.8996
-89.3979 105.3748 6.9699
-88.4582 104.4414 7.0389
-87.5667 103.4493 7.1064
-86.7222 102.4007 7.1724
-85.9230 101.2981 7.2369
-85.1674 100.1441 7.2998
-84.4536 98.9417 7.3609
-83.7795 97.6940 7.4203
-83.1427 96.4047 7.4779
-82.5409 95.0772 7.5336
-81.9715 93.7154 7.5874
-81.4318 92.3234 7.6392
-80.9191 90.9052 7.6889
-80.4304 89.4650 7.7365
-79.9629 88.0072 7.7819
-79.5136 86.5363 7.8251
-79.0794 85.0565 7.8660
-78.6574 83.5725 7.9047
-78.2444 82.0885 7.9409
-77.8375 80.6092 7.9748
-77.4337 79.1388 8.0062
-77.0299 77.6817 8.0352
-76.6235 76.2420 8.0616
-76.2114 74.8239 8.0855
-75.7912 73.4314 8.1068
-75.3600 72.0681 8.1256
-74.9155 70.7379 8.1418
-74.4552 69.4440 8.1553
-73.9769 68.1898 8.1662
-73.4786 66.9783 8.1745
-72.9582 65.8121 8.1802
-72.4140 64.6940 8.1832
-71.8445 63.6260 8.1836
-71.2480 62.6103 8.1813
-70.6234 61.6485 8.1764
-69.9696 60.7420 8.1690
-69.2856 59.8920 8.1590
-68.5707 59.0994 8.1464
-67.8243 58.3645 8.1313
-67.0460 57.6878 8.1137
-66.2356 57.0691 8.0936
-65.3931 56.5080 8.0711
-64.5185 56.0039 8.0462
-63.6122 55.5557 8.0189
-62.6745 55.1624 7.9894
-61.7061 54.8223 7.9576
-60.7076 54.5336 7.9236
-59.6799 54.2943 7.8875
-58.6240 54.1020 7.8493
-57.5409 53.9543 7.8090
-56.4319 53.8484 7.7668
-55.2982 53.7812 7.7227
-54.1413 53.7497 7.6768
-52.9624 53.7505 7.6291
-51.7632 53.7800 7.5797
-50.5453 53.8347 7.5287
-49.3101 53.9108 7.4761
-48.0594 54.0044 7.4221
-46.7947 54.1115 7.3667
-45.5178 54.2283 7.3099
-44.2303 54.3506 7.2520
-42.9339 54.4743 7.1928
-41.6301 54.5954 7.1326
-40.3206 54.7098 7.0714
-39.0069 54.8135 7.0094
-37.6904 54.9026 6.9464
-36.3727 54.9731 6.8828
-35.0551 55.0213 6.8185
-33.7388 55.0436 6.7537
-32.4252 55.0364 6.6883
-31.1153 54.9963 6.6226
-29.8101 54.9201 6.5566
-28.5106 54.8048 6.4904
-27.2175 54.6475 6.4241
-25.9318 54.4456 6.3577
-24.6538 54.1968 6.2914
-23.3842 53.8988 6.2252
-22.1233 53.5496 6.1592
-20.8714 53.1477 6.0936
-19.6287 52.6916 6.0284
-18.3953 52.1802 5.9636
-17.1710 51.6124 5.8995
-15.9558 50.9878 5.8360
-14.7494 50.3059 5.7733
-13.5515 49.5667 5.7114
-12.3616 48.7704 5.6504
-11.1793 47.9173 5.5904
-10.0039 47.0083 5.5315
-8.8348 46.0443 5.4738
-7.6713 45.0265 5.4172
-6.5127 43.9565 5.3620
-5.3581 42.8359 5.3082
-4.2066 41.6666 5.2559
-3.0576 40.4510 5.2050
-1.9100 39.1912 5.1558
-0.7631 37.8900 5.1082
0.3839 36.5499 5.0623
1.5320 35.1740 5.0181
2.6817 33.7654 4.9758
3.8339 32.3272 4.9354
4.9891 30.8627 4.8969
6.1479 29.3755 4.8603
7.3108 27.8690 4.8257
8.4781 26.3468 4.7932
9.6500 24.8128 4.7627
10.8267 23.2704 4.7342
12.0081 21.7236 4.7078
13.1940 20.1761 4.6835
14.3840 18.6316 4.6613
15.5776 17.0940 4.6411
16.7741 15.5669 4.6231
17.9726 14.0540 4.6070
19.1719 12.5590 4.5929
20.3707 11.0854 4.5809
21.5675 9.6367 4.5707
22.7606 8.2163 4.5624
23.9480 6.8275 4.5560
25.1275 5.4734 4.5512
26.2967 4.1571 4.5482
27.4529 2.8815 4.5467
28.5935 1.6494 4.5466
29.7153 0.4634 4.5480
30.8151 -0.6739 4.5506
31.8895 -1.7604 4.5543
32.9349 -2.7938 4.5591
33.9476 -3.7721 4.5647
34.9236 -4.6935 4.5711
35.8589 -5.5565 4.5780
36.7494 -6.3596 4.5853
37.5908 -7.1015 4.5928
38.3789 -7.7812 4.6004
39.1092 -8.3978 4.6079
39.7775 -8.9506 4.6151
40.3795 -9.4393 4.6218
40.9107 -9.8634 4.6278
41.3672 -10.2231 4.6330
41.7448 -10.5184 4.6370
42.0396 -10.7497 4.6398
42.2479 -10.9178 4.6410
42.3663 -11.0232 4.6406
42.3917 -11.0673 4.6384
42.3211 -11.0511 4.6340
42.1522 -10.9764 4.6273
41.8829 -10.8447 4.6182
41.5116 -10.6582 4.6065
41.0374 -10.4190 4.5919
40.4598 -10.1297 4.5743
39.7789 -9.7931 4.5535
38.9955 -9.4121 4.5295
38.1111 -8.9900 4.5020
37.1279 -8.5302 4.4709
36.0491 -8.0365 4.4361
34.8783 -7.5129 4.3975
33.6204 -6.9635 4.3550
32.2809 -6.3928 4.3085
30.8663 -5.8052 4.2580
29.3839 -5.2056 4.2035
27.8420 -4.5989 4.1448
26.2499 -3.9901 4.0821
24.6175 -3.3843 4.0153
22.9558 -2.7866 3.9444
21.2765 -2.2021 3.8695
19.5922 -1.6360 3.7908
17.9157 -1.0932 3.7082
16.2608 -0.5785 3.6220
14.6415 -0.0964 3.5323
13.0719 0.3489 3.4392
11.5663 0.7537 3.3430
10.1385 1.1146 3.2439
8.8020 1.4292 3.1423
7.5692 1.6953 3.0384
6.4513 1.9120 2.9327
5.4576 2.0791 2.8256
4.5952 2.1974 2.7177
3.8682 2.2689 2.6094
3.2769 2.2970 2.5015
2.8173 2.2862 2.3948
2.4798 2.2427 2.2901
2.2484 2.1741 2.1886
2.0994 2.0895 2.0914
2.0000 2.0000 2.0000
样本速度矩阵in2：
2.9432 -3.1313 0.0880
2.5472 -3.2035 0.0880
2.2109 -3.2545 0.0880
1.9270 -3.2861 0.0880
1.6890 -3.2998 0.0880
1.4912 -3.2972 0.0880
1.3284 -3.2795 0.0880
1.1963 -3.2479 0.0880
1.0910 -3.2035 0.0880
1.0091 -3.1473 0.0880
0.9475 -3.0803 0.0880
0.9035 -3.0036 0.0880
0.8750 -2.9179 0.0880
0.8598 -2.8243 0.0880
0.8561 -2.7235 0.0880
0.8623 -2.6164 0.0880
0.8770 -2.5040 0.0880
0.8990 -2.3869 0.0880
0.9272 -2.2660 0.0880
0.9606 -2.1422 0.0880
0.9983 -2.0161 0.0880
1.0394 -1.8886 0.0880
1.0833 -1.7604 0.0880
1.1294 -1.6322 0.0880
1.1770 -1.5047 0.0880
1.2255 -1.3785 0.0880
1.2746 -1.2544 0.0880
1.3236 -1.1329 0.0880
1.3722 -1.0146 0.0880
1.4200 -0.9000 0.0879
1.4666 -0.7896 0.0879
1.5117 -0.6839 0.0879
1.5549 -0.5833 0.0879
1.5959 -0.4882 0.0878
1.6346 -0.3990 0.0878
1.6705 -0.3158 0.0878
1.7036 -0.2391 0.0877
1.7336 -0.1689 0.0876
1.7602 -0.1055 0.0876
1.7835 -0.0491 0.0875
1.8031 0.0004 0.0874
1.8191 0.0429 0.0873
1.8313 0.0783 0.0871
1.8397 0.1066 0.0870
1.8442 0.1280 0.0868
1.8447 0.1426 0.0866
1.8414 0.1504 0.0864
1.8342 0.1516 0.0862
1.8232 0.1465 0.0859
1.8084 0.1352 0.0856
1.7900 0.1181 0.0853
1.7681 0.0953 0.0849
1.7427 0.0672 0.0845
1.7141 0.0340 0.0841
1.6824 -0.0037 0.0836
1.6478 -0.0459 0.0831
1.6104 -0.0920 0.0825
1.5706 -0.1416 0.0819
1.5286 -0.1945 0.0813
1.4845 -0.2501 0.0806
1.4386 -0.3082 0.0798
1.3913 -0.3683 0.0790
1.3426 -0.4299 0.0781
1.2930 -0.4928 0.0772
1.2426 -0.5565 0.0762
1.1917 -0.6205 0.0751
1.1407 -0.6846 0.0740
1.0897 -0.7483 0.0729
1.0390 -0.8113 0.0716
0.9889 -0.8731 0.0703
0.9397 -0.9335 0.0690
0.8915 -0.9921 0.0675
0.8446 -1.0486 0.0660
0.7992 -1.1026 0.0645
0.7555 -1.1540 0.0629
0.7138 -1.2024 0.0612
0.6741 -1.2476 0.0594
0.6368 -1.2894 0.0576
0.6018 -1.3275 0.0557
0.5694 -1.3618 0.0538
0.5397 -1.3921 0.0518
0.5127 -1.4182 0.0497
0.4886 -1.4402 0.0476
0.4675 -1.4578 0.0454
0.4493 -1.4710 0.0432
0.4342 -1.4797 0.0409
0.4221 -1.4841 0.0386
0.4130 -1.4839 0.0363
0.4069 -1.4793 0.0339
0.4038 -1.4704 0.0314
0.4037 -1.4571 0.0289
0.4065 -1.4397 0.0264
0.4120 -1.4181 0.0239
0.4203 -1.3926 0.0213
0.4312 -1.3632 0.0188
0.4445 -1.3303 0.0162
0.4603 -1.2939 0.0135
0.4783 -1.2542 0.0109
0.4983 -1.2116 0.0083
0.5204 -1.1661 0.0056
0.5442 -1.1182 0.0030
0.5696 -1.0680 0.0004
0.5964 -1.0157 -0.0022
0.6246 -0.9618 -0.0049
0.6538 -0.9065 -0.0075
0.6840 -0.8500 -0.0100
0.7149 -0.7927 -0.0126
0.7464 -0.7348 -0.0151
0.7783 -0.6767 -0.0176
0.8104 -0.6187 -0.0201
0.8425 -0.5611 -0.0225
0.8746 -0.5041 -0.0249
0.9063 -0.4481 -0.0272
0.9377 -0.3934 -0.0295
0.9684 -0.3401 -0.0318
0.9985 -0.2887 -0.0340
1.0277 -0.2393 -0.0361
1.0559 -0.1922 -0.0382
1.0831 -0.1477 -0.0402
1.1090 -0.1059 -0.0422
1.1337 -0.0672 -0.0441
1.1570 -0.0315 -0.0459
1.1788 0.0008 -0.0477
1.1992 0.0295 -0.0494
1.2180 0.0547 -0.0510
1.2352 0.0761 -0.0526
1.2507 0.0936 -0.0540
1.2646 0.1072 -0.0554
1.2769 0.1167 -0.0567
1.2875 0.1223 -0.0580
1.2964 0.1237 -0.0591
1.3038 0.1211 -0.0602
1.3095 0.1144 -0.0612
1.3137 0.1037 -0.0621
1.3164 0.0891 -0.0629
1.3177 0.0705 -0.0636
1.3176 0.0482 -0.0643
1.3162 0.0223 -0.0649
1.3136 -0.0072 -0.0653
1.3099 -0.0401 -0.0657
1.3052 -0.0762 -0.0660
1.2995 -0.1153 -0.0662
1.2930 -0.1573 -0.0663
1.2858 -0.2019 -0.0664
1.2780 -0.2489 -0.0663
1.2696 -0.2980 -0.0662
1.2609 -0.3491 -0.0659
1.2519 -0.4019 -0.0656
1.2427 -0.4561 -0.0652
1.2335 -0.5115 -0.0647
1.2242 -0.5677 -0.0641
1.2152 -0.6246 -0.0635
1.2064 -0.6819 -0.0627
1.1979 -0.7392 -0.0619
1.1899 -0.7964 -0.0610
1.1823 -0.8530 -0.0600
1.1754 -0.9090 -0.0589
1.1691 -0.9640 -0.0577
1.1635 -1.0178 -0.0565
1.1587 -1.0700 -0.0552
1.1546 -1.1206 -0.0538
1.1514 -1.1692 -0.0524
1.1491 -1.2157 -0.0508
1.1476 -1.2598 -0.0493
1.1469 -1.3013 -0.0476
1.1471 -1.3400 -0.0459
1.1480 -1.3759 -0.0441
1.1497 -1.4087 -0.0423
1.1522 -1.4382 -0.0404
1.1552 -1.4645 -0.0385
1.1588 -1.4872 -0.0366
1.1629 -1.5065 -0.0346
1.1673 -1.5221 -0.0326
1.1719 -1.5341 -0.0305
1.1767 -1.5423 -0.0285
1.1814 -1.5468 -0.0264
1.1859 -1.5475 -0.0243
1.1900 -1.5445 -0.0222
1.1936 -1.5376 -0.0202
1.1965 -1.5271 -0.0181
1.1985 -1.5129 -0.0161
1.1993 -1.4950 -0.0140
1.1988 -1.4736 -0.0121
1.1968 -1.4487 -0.0102
1.1931 -1.4204 -0.0083
1.1874 -1.3888 -0.0065
1.1795 -1.3541 -0.0047
1.1692 -1.3163 -0.0031
1.1563 -1.2756 -0.0015
1.1406 -1.2321 -0.0000
1.1218 -1.1860 0.0014
1.0998 -1.1374 0.0026
1.0744 -1.0864 0.0037
1.0454 -1.0334 0.0048
1.0127 -0.9783 0.0056
0.9760 -0.9215 0.0063
0.9353 -0.8630 0.0069
0.8905 -0.8031 0.0073
0.8414 -0.7419 0.0075
0.7881 -0.6797 0.0076
0.7304 -0.6166 0.0075
0.6683 -0.5528 0.0072
0.6019 -0.4886 0.0067
0.5313 -0.4242 0.0060
0.4565 -0.3597 0.0051
0.3776 -0.2953 0.0040
0.2948 -0.2314 0.0028
0.2083 -0.1680 0.0013
0.1184 -0.1055 -0.0004
0.0253 -0.0440 -0.0023
-0.0706 0.0161 -0.0044
-0.1689 0.0748 -0.0066
-0.2693 0.1317 -0.0091
-0.3712 0.1865 -0.0118
-0.4742 0.2392 -0.0146
-0.5776 0.2893 -0.0176
-0.6809 0.3366 -0.0207
-0.7834 0.3810 -0.0241
-0.8844 0.4221 -0.0275
-0.9831 0.4598 -0.0311
-1.0789 0.4937 -0.0348
-1.1707 0.5236 -0.0386
-1.2579 0.5494 -0.0425
-1.3395 0.5708 -0.0465
-1.4146 0.5875 -0.0505
-1.4824 0.5996 -0.0546
-1.5419 0.6067 -0.0586
-1.5922 0.6088 -0.0627
-1.6324 0.6058 -0.0668
-1.6617 0.5977 -0.0709
-1.6793 0.5844 -0.0748
-1.6844 0.5661 -0.0788
-1.6764 0.5428 -0.0826
-1.6549 0.5147 -0.0862
-1.6193 0.4821 -0.0897
-1.5696 0.4453 -0.0931
-1.5056 0.4048 -0.0962
-1.4278 0.3610 -0.0991
-1.3365 0.3145 -0.1016
-1.2328 0.2662 -0.1039
-1.1179 0.2167 -0.1057
-0.9937 0.1671 -0.1071
-0.8624 0.1183 -0.1080
-0.7270 0.0715 -0.1083
-0.5913 0.0281 -0.1079
-0.4596 -0.0108 -0.1067
-0.3375 -0.0435 -0.1047
-0.2314 -0.0686 -0.1015
-0.1490 -0.0845 -0.0972
-0.0994 -0.0895 -0.0914
1.0000 1.0000 1.0000
答案 0 :(得分：0)
观察矩阵H是先验（预测）状态向量x1与测量向量z之间的“链接”，因此z=H*x1 。因此观察矩阵是滤波器的参数，通常它不会改变。我们不知道您在测量和过滤哪些数据，因此我们无法详细说明矩阵H的形式以及您提供的其他矩阵。
PS。矩阵A的形式表明你是恒定速度运动的动力学模型，测量周期为0.1秒。矩阵M的形式建议测量3个量，可能是3D空间中的位置。如果这是正确的，您的测量矩阵应如下所示：
H=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0];
假设x1和z是列向量，它表示测量的预测只是由先验状态向量的前三个元素组成。