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