eadf00b11f695176039d6737e994b79e1ebbe448
[scilab.git] / scilab / modules / signal_processing / tests / unit_tests / kalm.dia.ref
1 // =============================================================================
2 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 // Copyright (C) 2013 - Scilab Enterprises - Charlotte Hecquet
4 // Copyright (C) 2013 - Scilab Enterprises - Sylvestre Ledru
5 //
6 //  This file is distributed under the same license as the Scilab package.
7 // =============================================================================
8 w=%pi/4; // angular frequency
9 T=0.1; // period
10 t=0:T:5
11  t  =
12  
13  
14          column  1 to 10
15  
16     0.    0.1    0.2    0.3    0.4    0.5    0.6    0.7    0.8    0.9  
17  
18          column 11 to 20
19  
20     1.    1.1    1.2    1.3    1.4    1.5    1.6    1.7    1.8    1.9  
21  
22          column 21 to 30
23  
24     2.    2.1    2.2    2.3    2.4    2.5    2.6    2.7    2.8    2.9  
25  
26          column 31 to 40
27  
28     3.    3.1    3.2    3.3    3.4    3.5    3.6    3.7    3.8    3.9  
29  
30          column 41 to 50
31  
32     4.    4.1    4.2    4.3    4.4    4.5    4.6    4.7    4.8    4.9  
33  
34          column 51
35  
36     5.  
37 signal=cos(w*t);
38 // Sinusoid with noise
39 v=0:1:50
40  v  =
41  
42  
43          column  1 to 11
44  
45     0.    1.    2.    3.    4.    5.    6.    7.    8.    9.    10.  
46  
47          column 12 to 21
48  
49     11.    12.    13.    14.    15.    16.    17.    18.    19.    20.  
50  
51          column 22 to 31
52  
53     21.    22.    23.    24.    25.    26.    27.    28.    29.    30.  
54  
55          column 32 to 41
56  
57     31.    32.    33.    34.    35.    36.    37.    38.    39.    40.  
58  
59          column 42 to 51
60  
61     41.    42.    43.    44.    45.    46.    47.    48.    49.    50.  
62 y=signal+v;
63 // System
64 n=2; // system order
65 f=[cos(w*T) -sin(w*T); sin(w*T) cos(w*T)];
66 g=0;
67 h=[1 0];
68 p0=[1000 0; 0 0];
69 R=1;
70 Q=0;
71 x0=zeros(n,1);
72 // Initialize for loop
73 x1=x0;
74 p1=p0;
75 // Kalman filter
76 xsum=0;
77 x1sum=0;
78 psum=0;
79 p1sum=0;
80 for i=1:length(t)-1
81     [x1(:,i+1),p1,x,p]=kalm(y(i),x1(:,i),p1,f,g,h,Q,R);
82     xsum = xsum + x;
83     p1sum = p1sum + p1;
84     psum = psum + p;
85     x1sum = x1sum + x1(:,i+1);
86 end
87 assert_checkalmostequal(xsum, [295.374859628719719;202.134913816696837]);
88 assert_checkalmostequal(p1sum,  [3.88430845743643571,0.63055345519216977;0.63055345519217121,2.08824467576958606]);
89 assert_checkalmostequal(psum, [3.97189248710404863,0.48230715738369057;0.48230715738369184,2.0006606461019758]);
90 assert_checkalmostequal(x1sum,  [278.60499495977632;224.686643723725581]);