Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Fredrik Bagge Carlson
SystemIdentification
Commits
2d7d3c96
Commit
2d7d3c96
authored
Dec 01, 2015
by
Mattias Fält
Browse files
Initial SQMC commit with own PF
parent
9fcbbe36
Changes
3
Show whitespace changes
Inline
Side-by-side
src/hilbert_curve.jl
0 → 100644
View file @
2d7d3c96
#Convert number in [0,1)^2 to [0,1)
function
xy2d
(
x
::
Float64
,
y
::
Float64
)
N
=
2
^
30
xint
=
floor
(
Int64
,
x
*
N
)
yint
=
floor
(
Int64
,
y
*
N
)
# d will be in [0,N^2)
d
=
xy2d
(
N
,
xint
,
yint
)
/
N
^
2
end
function
d2xy
(
d
::
Float64
)
N
::
Int64
=
2
^
30
dint
=
floor
(
Int64
,
d
*
N
^
2
)
x
::
Float64
,
y
::
Float64
=
d2xy
(
N
^
2
,
dint
)
x
/
N
,
y
/
N
end
#Converted from wikipedia
#convert (x,y) to d
function
xy2d
(
n
::
Integer
,
x
::
Integer
,
y
::
Integer
)
rx
,
ry
,
s
,
d
=
0
,
0
,
0
,
0
s
=
div
(
n
,
2
)
while
s
>
0
rx
=
(
x
&
s
)
>
0
ry
=
(
y
&
s
)
>
0
d
+=
s
*
s
*
((
3
*
rx
)
$
ry
)
x
,
y
=
rot
(
s
,
x
,
y
,
rx
,
ry
)
s
=
div
(
s
,
2
)
end
return
d
::
Integer
end
#convert d to (x,y)
function
d2xy
(
n
::
Integer
,
d
::
Integer
)
rx
,
ry
,
s
,
t
=
0
,
0
,
0
,
d
x
,
y
=
0
,
0
s
=
1
while
s
<
n
rx
=
1
&
(
div
(
t
,
2
))
ry
=
1
&
(
t
$
rx
)
x
,
y
=
rot
(
s
,
x
,
y
,
rx
,
ry
)
x
+=
s
*
rx
y
+=
s
*
ry
t
=
div
(
t
,
4
)
s
*=
2
end
return
x
::
Integer
,
y
::
Integer
end
#rotate/flip a quadrant appropriately
function
rot
(
n
::
Integer
,
x
::
Integer
,
y
::
Integer
,
rx
::
Integer
,
ry
::
Integer
)
if
ry
==
0
if
rx
==
1
x
=
n
-
1
-
x
y
=
n
-
1
-
y
end
#Swap x and y
t
=
x
x
=
y
y
=
t
end
return
x
::
Integer
,
y
::
Integer
end
src/particle_filters/SQMC_test.jl
0 → 100644
View file @
2d7d3c96
using
Gadfly
using
Colors
function
GordonKitagawaUpdate!
(
x
,
t
,
u
,
xtemp
,
i
)
b₁
,
b₂
,
b₃
,
b₄
,
σ
=
.
5
,
25
,
8
,
1.2
,
sqrt
(
1
);
xtemp
[
1
,
i
]
=
b₁
*
x
+
b₂
*
x
/
(
t
+
x
^
2
)
+
b₃
*
cos
(
b₄
*
t
)
+
σ
*
erfinv
(
u
[
1
]
*
2
-
1
);
end
function
GordonKitagawaOut
(
x
)
σ
=
sqrt
(
.
1
);
a
=
20
x
^
2
/
a
+
σ
*
randn
()[
1
]
end
function
GordonKitagawaOutNoNoise
(
x
)
a
=
20
x
.^
2.
/
a
end
function
GordonKitagawapxy
(
yhat
,
y
)
σ
=
sqrt
(
.
1
);
#w = 1/sqrt(2π)*e^(-(yhat[1]-y[1])^2/2)
w
=
-
(
yhat
[
1
]
-
y
[
1
])
^
2
/
(
2
*
σ
^
2
)
end
function
generateRealSequence
(
f!
,
g
,
x0
,
T
,
N
=
length
(
x0
))
x
=
Array
{
Float64
,
2
}(
length
(
x0
),
T
)
f!
(
x0
,
1
,
rand
(
N
),
x
,
1
)
y1
=
g
(
x
[
:
,
1
],
1
)
y
=
Array
{
Float64
,
2
}(
length
(
y1
),
T
)
y
[
:
,
1
]
=
y1
for
t
=
2
:
T
f!
(
x
[
t
-
1
],
t
,
rand
(
N
),
x
,
t
)
y
[
:
,
t
]
=
g
(
x
[
t
],
t
)
end
x
,
y
end
function
estMean
(
x
,
w
,
t
)
sum
(
x
[
:
]
.*
exp
(
w
[
:
]))
end
function
runTest
(
N
,
method
,
debug
)
T
=
150
##Gordon Kitagawa
#f! = (x,t,u,xtemp,i) -> GordonKitagawaUpdate!(x[1],t,u,xtemp,i)
#g = (x,t) -> GordonKitagawaOutNoNoise(x[1])
#gn = (x,t) -> GordonKitagawaOut(x[1])
#ginv = (yhat, y,t) -> GordonKitagawapxy(yhat,y)
##LTI System
σ
=
.
5
f!
=
(
x
,
t
,
u
,
xtemp
,
i
)
->
xtemp
[
1
,
i
]
=
.
8
x
[
1
]
+
4
*
erfinv
(
u
[
1
]
*
2
-
1
)
g
=
(
x
,
t
)
->
2
*
x
[
1
]
gn
=
(
x
,
t
)
->
2
*
x
[
1
]
+
σ
*
randn
()[
1
]
ginv
=
(
yhat
,
y
,
t
)
->
-
(
yhat
[
1
]
-
y
[
1
])
^
2
/
(
2
*
σ
^
2
)
##Estimator
#est = (x,w,t) -> x[findmax(w)[2]]
est
=
(
x
,
w
,
t
)
->
sum
(
x
[
:
]
.*
exp
(
w
[
:
]))
x0
=
.
5
x
,
y
=
generateRealSequence
(
f!
,
gn
,
x0
,
T
)
xhat
=
method
(
f!
,
g
,
ginv
,
x0
,
y
,
est
,
N
,
debug
=
debug
,
xreal
=
x
)
x
,
xhat
end
function
plotPoints
(
x
,
w
,
y
,
N
,
a
,
τ
,
t
,
xreal
,
xhat
)
c
=
w
[
:
]
-
minimum
(
w
)
+
1
##Use for GordonK
#p = plot(layer(x=collect(1:N), y=x[:], Geom.point, color=c),
# layer(x=[1,N],y=ones(2).*sqrt(20*max(y[:,t],0)),Geom.line, Theme(default_color=color(colorant"red"))),
# layer(x=[1,N],y=-ones(2).*sqrt(20*max(y[:,t],0)),Geom.line, Theme(default_color=color(colorant"red"))),
# layer(x=[1,N],y=ones(2).*xreal[:,t],Geom.line, Theme(default_color=color(colorant"blue"),line_width=2px)),
# layer(x=[1,N],y=ones(2).*xhat[:,t],Geom.line, Theme(default_color=color(colorant"black"),line_width=4px)),
# Guide.XLabel("Particle "*string(t)), Guide.YLabel("Estimate"), Coord.Cartesian(ymin=-15,ymax=15))
##Use for LTI
p
=
plot
(
layer
(
x
=
collect
(
1
:
N
),
y
=
x
[
:
],
Geom
.
point
,
color
=
c
),
layer
(
x
=
[
1
,
N
],
y
=
ones
(
2
)
*
1
/
2.
*
y
[
:
,
t
],
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"red"
))),
layer
(
x
=
[
1
,
N
],
y
=
ones
(
2
)
.*
xreal
[
:
,
t
],
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"blue"
),
line_width
=
2
px
)),
layer
(
x
=
[
1
,
N
],
y
=
ones
(
2
)
.*
xhat
[
:
,
t
],
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"black"
),
line_width
=
4
px
)),
Guide
.
XLabel
(
"Particle "
*
string
(
t
)),
Guide
.
YLabel
(
"Estimate"
),
Coord
.
Cartesian
(
ymin
=-
10
,
ymax
=
10
))
display
(
p
)
print
(
"here"
)
readline
(
STDIN
)
end
function
rms
(
x
)
sqrt
(
1
/
length
(
x
)
*
sum
(
x
.^
2
))
end
function
testSQMC
()
debug
=
false
Ns
=
2.
^
(
2
:
11
)
M
=
50
RMS
=
Array
{
Float64
,
2
}(
length
(
Ns
),
M
)
largeRMS
=
Array
{
Float64
}(
length
(
Ns
),
2
)
rmsMean
=
Array
{
Float64
,
2
}(
length
(
Ns
),
2
)
rmsVariance
=
Array
{
Float64
,
2
}(
length
(
Ns
),
2
)
for
(
methodidx
,
method
)
in
enumerate
([
SQMC
,
pf
])
xhat
,
xreal
=
0
,
0
@time
for
(
i
,
N
)
in
enumerate
(
Ns
)
rmslocal
=
Array
{
Float64
,
1
}(
M
)
for
j
=
1
:
M
xreal
,
xhat
=
runTest
(
N
,
method
,
debug
)
RMS
[
i
,
j
]
=
rms
(
xreal
-
xhat
)
end
end
rmsMean
[
:
,
methodidx
]
=
mean
(
RMS
,
2
)
rmsVariance
[
:
,
methodidx
]
=
std
(
RMS
,
2
)
./
sqrt
(
M
)
for
(
i
,
N
)
in
enumerate
(
Ns
)
largeRMS
[
i
,
methodidx
]
=
length
(
find
(
RMS
[
i
,
:
]
.>
rmsMean
[
i
,
methodidx
]
+
2
*
rmsVariance
[
i
,
methodidx
]))
end
end
rmsMean
,
rmsVariance
,
Ns
,
largeRMS
,
RMS
end
function
testPlot
(
rmsMean
,
rmsVariance
,
Ns
)
p
=
plot
(
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
1
],
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"red"
))),
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
2
],
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"blue"
))),
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
1
]
+
rmsVariance
[
:
,
1
]
*
2
,
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"red"
))),
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
1
]
-
rmsVariance
[
:
,
1
]
*
2
,
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"red"
))),
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
2
]
+
rmsVariance
[
:
,
2
]
*
2
,
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"blue"
))),
layer
(
x
=
Ns
,
y
=
rmsMean
[
:
,
2
]
-
rmsVariance
[
:
,
2
]
*
2
,
Geom
.
line
,
Theme
(
default_color
=
color
(
colorant
"blue"
))),
)
end
function
testPlot
(
N
)
for
j
=
1
:
N
M
=
1000
vals
=
Array
{
Float64
,
2
}(
M
,
2
)
ds
=
linspace
(
rand
()
/
M
,
1
,
M
)[
1
:
end
]
for
i
=
1
:
M
o
=
d2xy
(
ds
[
i
])
vals
[
i
,
:
]
=
[
o
[
1
],
o
[
2
]]
end
plot
(
vals
[
:
,
1
],
vals
[
:
,
2
])
end
end
function
testPlot2
(
N
)
for
j
=
1
:
N
M
=
1000
vals
=
Array
{
Float64
,
2
}(
M
,
2
)
ds
=
sort
(
rand
(
M
))
for
i
=
1
:
M
o
=
d2xy
(
ds
[
i
])
vals
[
i
,
:
]
=
[
o
[
1
],
o
[
2
]]
end
plot
(
vals
[
:
,
1
],
vals
[
:
,
2
])
end
end
src/particle_filters/pf_SQMC.jl
0 → 100644
View file @
2d7d3c96
function
resample
(
s
,
w
)
# Samples new particles based on their weights. If you find algorithmic optimizations to this routine, please tell me /Bagge)
N
=
length
(
w
)
bins
=
[
0.
;
cumsum
(
exp
(
w
))]
j
=
zeros
(
Int64
,
N
)
bo
=
1
for
i
=
1
:
N
for
b
=
bo
:
N
if
bins
[
b
]
<=
s
[
i
]
# < bins[b+1]
j
[
i
]
=
b
bo
=
b
break
end
end
end
return
j
end
function
resample
(
w
)
bins
=
[
0.
;
cumsum
(
exp
(
w
))]
s
=
collect
((
rand
()
/
N
+
0
)
:
1
/
N
:
bins
[
end
])
resample
(
s
,
w
)
end
@doc
""" `xhat = SQMC(f, g, ginv, x0, y, est, N=200, Nu=lenth(x0))`
Gives estimation of the states `x(:,1),...x(:,T)` given outputs `y(:,1)...y(:,T)` from system:
x(t) = f(x(t-1),t,u)
\n
y = gn(x,v),
\n
where `g(x,t)=E[gn(x,t,u)]`, `ginv(yhat, y, t) = p(y=gn(x,u)|yhat=g(x))`,
u is uniform noise on `[0,1)`, with `E[x(0)] = xhat0`,
and some estimator `est(xₚ,w,t)` that ouputs the estimate `xhat(:,t)`
given the particles `xₚ` and weights `w`
"""
->
function
SQMC
(
f!
,
g
,
ginv
,
xhat0
,
y
,
est
,
N
=
200
,
Nu
=
length
(
xhat0
);
debug
=
false
,
xreal
=
xreal
)
T
=
size
(
y
,
2
)
#Estimates at time 1:T
xhat
=
Array
{
Float64
,
2
}(
Nu
,
T
)
#Current predicted output for each particle
yhat
=
Array
{
Float64
,
2
}(
size
(
y
,
1
),
N
)
x
=
Array
{
Float64
,
2
}(
length
(
xhat0
),
N
)
xtemp
=
similar
(
x
)
w
=
Array
{
Float64
,
1
}(
N
)
#Step (a) (Draw u and x), OBS u will be of size NxNu
u
,
=
sobol
(
Nu
,
N
)
x
[
:
,
1
]
=
xhat0
;
#Hack to use the pfStep fuction. Works by letting `a` be ones
pfStep!
(
f!
,
g
,
ginv
,
x
,
xtemp
,
u
,
ones
(
Int
,
N
),
1
:
N
,
1
,
w
,
yhat
,
y
,
N
,
1
)
xhat
[
:
,
1
]
=
est
(
x
,
w
,
1
)
u
,
nextseed
,
MeM
=
sobol
(
Nu
+
1
,
N
)
for
t
=
2
:
T
nextseed
=
sobol!
(
u
,
nextseed
,
MeM
)
τ
=
sortperm
(
u
[
:
,
1
])
σ
=
sortperm
(
ψ
(
x
)[
:
])
a
=
resample
(
u
[
τ
,
1
],
w
[
σ
])
# Time update, no sigma here?
pfStep!
(
f!
,
g
,
ginv
,
x
,
xtemp
,
u
,
a
,
τ
,
t
,
w
,
yhat
,
y
,
N
,
2
:
Nu
+
1
)
xhat
[
:
,
t
]
=
est
(
x
,
w
,
1
)
if
debug
plotPoints
(
x
,
w
,
y
,
N
,
a
,
τ
,
t
,
xreal
,
xhat
)
end
end
xhat
end
function
ψ
(
x
)
x
end
function
pf
(
f!
,
g
,
ginv
,
xhat0
,
y
,
est
,
N
=
200
,
Nu
=
length
(
xhat0
);
debug
=
false
,
xreal
=
xreal
)
T
=
size
(
y
,
2
)
#Estimates at time 1:T
xhat
=
Array
{
Float64
,
2
}(
Nu
,
T
)
#Current predicted output for each particle
yhat
=
Array
{
Float64
,
2
}(
size
(
y
,
1
),
N
)
x
=
Array
{
Float64
,
2
}(
length
(
xhat0
),
N
)
xtemp
=
similar
(
x
)
w
=
Array
{
Float64
,
1
}(
N
)
τ
=
1
:
N
u
=
rand
(
N
,
Nu
)
x
[
:
,
1
]
=
xhat0
;
#Hack to use the pfStep fuction. Works by letting `a` be ones
pfStep!
(
f!
,
g
,
ginv
,
x
,
xtemp
,
u
,
ones
(
Int
,
N
),
τ
,
1
,
w
,
yhat
,
y
,
N
,
1
:
Nu
)
xhat
[
:
,
1
]
=
est
(
x
,
w
,
1
)
for
t
=
2
:
T
u
=
rand
(
N
,
Nu
)
a
=
resample
(
u
[
τ
,
1
],
w
[
:
])
# Time update, no sigma here?
pfStep!
(
f!
,
g
,
ginv
,
x
,
xtemp
,
u
,
a
,
τ
,
t
,
w
,
yhat
,
y
,
N
,
1
:
Nu
)
xhat
[
:
,
t
]
=
est
(
x
,
w
,
1
)
end
xhat
end
#Time update and weighting. Chances `x`, `yhat` and `w`
@inbounds
function
pfStep!
(
f
,
g
,
ginv
,
x
,
xtemp
,
u
,
a
,
τ
,
t
,
w
,
yhat
,
y
,
N
,
uRange
)
for
i
=
1
:
N
f
(
x
[
:
,
a
[
i
]],
t
,
u
[
τ
[
i
],
uRange
],
xtemp
,
i
)
yhat
[
:
,
i
]
=
g
(
xtemp
[
:
,
i
],
t
)
w
[
i
]
=
ginv
(
yhat
[
:
,
i
],
y
[
:
,
t
],
t
)
end
x
[
:
]
=
copy
(
xtemp
)
offset
=
maximum
(
w
)
mySum
=
sum
(
exp
(
w
-
offset
))
normConstant
=
log
(
mySum
)
+
offset
w
[
:
]
=
w
[
:
]
-
normConstant
end
\ No newline at end of file
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment