1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
|
////////////////////////
//WIENER FILTERING//////
////////////////////////
// Copyright INRIA
xbasc();xselect()
//define system macro which generates the next
//observation given the old state
deff('[x1,y]=system(x0,f,g,h,q,r)',[
'rand(''normal'');'
'q2=chol(q);'
'r2=chol(r);'
'u=q2''*rand(ones(x0));'
'v=r2''*rand(ones(x0));'
'x1=f*x0+g*u;'
'y=h*x1+v;'])
//initialize state statistics (mean and error variance)
m0=[10;10];
p0=[100 0;0 100];
//create system
f=[1.15 .1;0 .8];
g=[1 0;0 1];
h=[1 0;0 1];
[hi,hj]=size(h);
//noise statistics
q=[.01 0;0 .01];
r=20*eye(2,2);
//initialize system process
rand('seed',66),
rand('normal'),
p0c=chol(p0);
x0=m0+p0c'*rand(ones(m0));
y=h*x0+chol(r)'*rand(ones(1:hi))';
yt=y;
//initialize plotted variables
x=x0; ft=f; gt=g; ht=h; qt=q; rt=r;
n=10;
for k=1:n,
//generate the state and observation at time k (i.e. xk and yk)
[x1,y]=system(x0,f,g,h,q,r);
x=[x x1];x0=x1;
yt=[yt y];ft=[ft f];gt=[gt g];ht=[ht h];qt=[qt q];rt=[rt r];
end,
//get the wiener filter estimate
[xs,ps,xf,pf]=wiener(yt,m0,p0,ft,gt,ht,qt,rt);
//plot result
//plot frame, real state (x), and estimates (xf, and xs)
plot2d([x(1,:)',xf(1,:)',xs(1,:)'],..
[x(2,:)',xf(2,:)',xs(2,:)'],[1 2 3],"161",..
'real state@estimates xf@estimates xs'),
//mark data points (* for real data, o for estimates)
plot2d([x(1,:)',xf(1,:)',xs(1,:)'],..
[x(2,:)',xf(2,:)',xs(2,:)'],-[1 2 3],"000",..
'real state@estimates xf@estimates xs'),
|
