Assumptions of initialization of Kalman Filter Matlab
Question
I'm designing a Kalman Filter in Matlab, However I want to set initial values. I have set following matrices and finding a way to set initialize observation.
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
I want to set initial observation as I have Actual initial conditions in Matrix X, what is the initialization of assumptions of observation matrix and the rest.
EDIT 1: I'v implemented EKF (Extended Kalman Filter) in Matlab for Visual Tracking of Object's 3D trajectory, However, I'm giving it actual trajectory's position and velocity as in1 and in2 respectively. I'm facing wrong prediction after some points which is usually opposite to the actual trajectory. What I think, it may be some initial assumptions problem as I'v checked the equations many times but unable to find the bug. How to fix my initial assumptions or to avoid by any other guesses? Sample position and velocity are given. Any suggestions for improving my approach are welcomed. My Code:
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
Sample position matrix 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
Sample velocity Matrix 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
1 answers
solution1
0 2014-05-18 21:33:32
Observation matrix H is "link" between a priori (predicted) state vector x1 and measurement vector z , such that z=H*x1 . So the observation matrix is a parameter of filter and generally it doesn't change. We don't know what data are you measuring and filtering, so we can't say much about form of matrix H and other matrices you provided.
PS. The form of matrix A suggest you're modelling dynamics of constant velocity motion with measurement period of 0.1 second. The form of matrix M suggest measuring 3 quantities, probably position in 3D space. If that is correct, your measurement matrix should look like that:
H=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0];
Assuming x1 and z are column vector, it says that prediction of measurement is simply composed from first three elements of a priori state vector.
The technical post webpages of this site follow the CC BY-SA 4.0 protocol. If you need to reprint, please indicate the site URL or the original address.Any question please contact:yoyou2525@163.com.
Related Question
matlab kalman filter coding
Matlab kalman filter in python
Numerical Instability Kalman Filter in MatLab
Kalman filter, car tracking, Matlab
improving computationnal speed of a kalman filter in Matlab
Implementing a Kalman Filter in MATLAB using 'ss'
input position/velocity to kalman filter matlab
Custom motion estimation model for Kalman filter in MATLAB
Error state Kalman Filter from MATLAB to Python
Matlab Kalman Filter Code - Why not working?
Related Tags