Backport https://codereview.scilab.org/#/c/14613/ tests to 5.5 branch
[scilab.git] / scilab / modules / signal_processing / tests / unit_tests / kalm.tst
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
9 w=%pi/4; // angular frequency
10 T=0.1; // period
11 t=0:T:5;
12 signal=cos(w*t);
13 // Sinusoid with noise
14 v=0:1:50;
15 y=signal+v;
16 // System
17 n=2; // system order
18 f=[cos(w*T) -sin(w*T); sin(w*T) cos(w*T)];
19 g=0;
20 h=[1 0];
21 p0=[1000 0; 0 0];
22 R=1;
23 Q=0;
24 x0=zeros(n,1);
25 // Initialize for loop
26 x1=x0;
27 p1=p0;
28 // Kalman filter
29 xsum=0;
30 x1sum=0;
31 psum=0;
32 p1sum=0;
33 for i=1:length(t)-1
34     [x1(:,i+1),p1,x,p]=kalm(y(i),x1(:,i),p1,f,g,h,Q,R);
35     xsum = xsum + x;
36     p1sum = p1sum + p1;
37     psum = psum + p;
38     x1sum = x1sum + x1(:,i+1);
39 end
40 assert_checkalmostequal(xsum, [295.374859628719719;202.134913816696837]);
41
42 assert_checkalmostequal(p1sum,  [3.88430845743643571,0.63055345519216977;0.63055345519217121,2.08824467576958606]);
43
44 assert_checkalmostequal(psum, [3.97189248710404863,0.48230715738369057;0.48230715738369184,2.0006606461019758]);
45 assert_checkalmostequal(x1sum,  [278.60499495977632;224.686643723725581]);
46