diff --git a/jump_lin_id/two_link.jl b/jump_lin_id/two_link.jl
index 9112d54b8712369686e187d186b88477f6e2a112..77022fd363f361539af3d99aa59b847b7b9ff341 100644
--- a/jump_lin_id/two_link.jl
+++ b/jump_lin_id/two_link.jl
@@ -14,7 +14,7 @@ if !isdefined(:AbstractEnsembleSystem)
@everywhere include("../flux/nn_prior/linear_sys.jl")
end
-n,m,N,h,σ0 = 4,3,200,0.02,0.01
+n,m,N,h,σ0 = 4,3,200,0.02,0.0
np = n*(n+m)
sys = TwoLinkSys(N=N, h=h, σ0 = σ0)#; warn("Added noise")
seed = 1
@@ -26,20 +26,20 @@ p = forward_kin(q)
qd = centraldiff(q')'/sys.h
qdd = centraldiff(qd')'/sys.h
u = torque(q,qd,qdd)
-u .+= 0.1randn(size(u))
+u .+= 0.2randn(size(u))
# x,y,u = generate_data(sys, System, seed; ufun=x->u)
const wall = -0.2
function affect!(i)
- q = i.u[1:2]
- qd = i.u[3:4]
- p1,p2 = forward_kin(q)[2]
- J = TwoLink.jacobian(q)
- v1,v2 = J*qd
- d = [0, wall-p2]
- δq = J\d
- i.u[3:4] = J\[v1, 0]
- i.u[1:2] .+= δq
+ # q = i.u[1:2]
+ # qd = i.u[3:4]
+ # p1,p2 = forward_kin(q)[2]
+ # J = TwoLink.jacobian(q)
+ # v1,v2 = J*qd
+ # d = [0, wall-p2]
+ # δq = J\d
+ # i.u[3:4] = J\[v1, 0]
+ # i.u[1:2] .+= δq
end
condition(x,t,i) = forward_kin(x)[2][2] > wall
cb = DiscreteCallback(condition,affect!)
@@ -68,7 +68,7 @@ Jtrue = reshape(Jtrue, 24, sys.N)
colors = [HSV(h,1,0.8) for h in linspace(0,200,5)]
-sλ = linspace(-5,5,20)
+sλ = linspace(-5,5,5)
sr2 = linspace(-4,4,20)
sp0 = linspace(-3,8,20)
@everywhere using ParallelDataTransfer
@@ -161,50 +161,67 @@ pgfplots(grid=false, legend=:bottom)
##
-r2,p0 = -3.,8.
-R1 = eye(n*(n+m))
-R2 = 10^r2*eye(n)
+p0 = 1.
+R1 = eye(np)
+R2 = 10^(1.)*eye(n)
P0 = 10^p0*R1
-model = LTVModels.fit_model(LTVModels.KalmanModel, x,u,R1,R2,P0, extend=true)
+
+sx = std(x,2)
+su = std(u[1:2,:])
+mx, mu = mean(x,2), mean(u[1:2,:],2)
+xt = (x.-mx) ./ sx
+ut = copy(u)
+ut[1:2,:] = (u[1:2,:].-mu) ./ su
+
+Pt = cat(3,[diagm([fill(10^4,n^2);fill(10^10,n*m)]) for i=1:N]...)
+model = KalmanModel(zeros(n,n,N),zeros(n,m,N),zeros(1,1,N),false)
+prior = KalmanModel(zeros(n,n,N),zeros(n,m,N),Pt,false)
+model = LTVModels.fit_model!(model,prior, xt,ut,R1,R2,P0, extend=true)
signch = findfirst(u[2,:] .< 0)
contact = findfirst(forward_kin(x[1:2,:])[2,:] .> wall)
ind2 = length(sp0)
-fig=plot(x', label="States", show=false, layout=grid(3,2), subplot=1:4, size=(800,1000), title="State trajectories", title=["q1" "q2" "qd1" "qd2"], c=:blue, linewidth=4)
+fig=plot(xt', label="States", show=false, layout=grid(3,2), subplot=1:4, size=(1800,1000), title="State trajectories", title=["q1" "q2" "qd1" "qd2"], c=:blue, linewidth=4)
vline!([signch].*ones(1,4), show=false, subplot=1:4, lab="Velocity sign change", l=(:black, :dash))
vline!([contact].*ones(1,4), show=false, subplot=1:4, lab="Stiff contact", l=(:black, :dash))
plot!(forward_kin(x[1:2,:])', subplot=5, title="End effector positions", lab=["x" "y"])
hline!([wall], l=(:dash, :black), grid=false, subplot=5, lab="Constraint")
vline!([contact], subplot=5, lab="Stiff contact", l=(:black, :dash))
-plot!(u[1:2,:]', subplot=6, title="Control torques", lab="")
+plot!(ut[1:2,:]', subplot=6, title="Control torques", lab="")
vline!([signch], subplot=6, lab="Velocity sign change", l=(:black, :dash))
-plot!(LTVModels.simulate(model, x[:,1], u)', label="Simulation", show=false, subplot=1:4, l=(:red,), legend=[false false true false])
+plot!(LTVModels.simulate(model, xt[:,1], ut)', label="Simulation", show=false, subplot=1:4, l=(:red,), legend=[false false true false])
gui()
# savefig2(@__DIR__()*"/figs/robot_train.tex", Dict("" => "legend style={at={(1.2,-0.2)}, anchor=north, legend columns=4},"))
##
# Validate
##
-let u, x, y
-u = torque(q,qd,qdd)
-u .+= 0.1randn(size(u))
+
+ # @views model.At[:,:,:] .*= 0.99
+uv = torque(q,qd,qdd)
+uv .+= 0.0randn(size(uv))
t = h:h:N*h+h
q0 = [q[:,1]; qd[:,1]]
-prob = OrdinaryDiffEq.ODEProblem((x,p,t)->time_derivative(x, u[:,ceil(Int,t/h)]),q0,(t[[1,end]]...))
+prob = OrdinaryDiffEq.ODEProblem((x,p,t)->time_derivative(x, uv[:,ceil(Int,t/h)]),q0,(t[[1,end]]...))
sol = solve(prob,Tsit5(),reltol=1e-8,abstol=1e-8, callback=cb)
-x = hcat(sol(t)...)
-u = u[:,1:N]
-u = [u; ones(1,size(u,2))] # Extend to estimate friction
-x,y = xy(System,x,N)
+xv = hcat(sol(t)...)
+uv = uv[:,1:N]
+uv = [uv; ones(1,size(uv,2))] # Extend to estimate friction
+xv,yv = xy(System,xv,N)
-plot(x', label="States", show=false, layout=grid(2,2), subplot=1:4, size=(800,1000), title="State trajectories", title=["q1" "q2" "qd1" "qd2"], c=:blue, linewidth=4, reuse=false)
+xt = (xv.-mx) ./ sx
+ut = copy(uv)
+ut[1:2,:] = (uv[1:2,:].-mu) ./ su
+
+plot(xt', label="States", show=false, layout=grid(2,2), subplot=1:4, size=(1800,1000), title="State trajectories", title=["q1" "q2" "qd1" "qd2"], c=:blue, linewidth=4, reuse=false)
vline!([signch].*ones(1,4), show=false, subplot=1:4, lab="Original Velocity sign change", l=(:black, :dash))
vline!([contact].*ones(1,4), show=false, subplot=1:4, lab="Original Stiff contact", l=(:black, :dash))
-plot!(LTVModels.simulate(model, x[:,1], u)', label="Simulation", show=false, subplot=1:4, l=(:red,), legend=[false false true false])
+plot!(LTVModels.simulate(model, xt[:,1], ut)', label="Simulation", show=false, subplot=1:4, l=(:red,), legend=[false false true false])
+# plot!(ut', subplot=5:6)
gui()
# savefig2(@__DIR__()*"/figs/robot_val.tex", Dict("" => "legend style={at={(1.2,-0.2)}, anchor=north, legend columns=4},"))
-end
+
##
diff --git a/pf_vec.jl b/pf_vec.jl
index 91b47f9a56dae18173f37767d2f43ee5593ee083..de4175430579685a0bb29379e403d69427b69b1d 100644
--- a/pf_vec.jl
+++ b/pf_vec.jl
@@ -18,6 +18,7 @@ function pf!(x, xprev, w, u, y, g, f)
end
g(w,y,x)
logsumexp!(w)
+ copy!(xprev, x)
x
end
@@ -102,32 +103,37 @@ function run_test()
for (Ni, N) in enumerate(particle_count)
montecarlo_runs = maximum(particle_count)*maximum(time_steps) / T / N
# montecarlo_runs = 1
- xp = Array{Float64}(N,T)
- w = Array{Float64}(N,T)
- x = Array{Float64}(T)
- y = Array{Float64}(T)
-
- E = @parallel (+) for mc_iter = 1:montecarlo_runs
- x[1] = σw*randn()
- y[1] = σv*randn()
- @inbounds for t = 1:T-1
- x[t+1] = f(x[t],u) + σw*randn()
- y[t+1] = 0.05x[t+1]^2 + σv*randn()
- end # t
- xh = pf!(x, xprev, w, u, y, g, f)
- rms(x-xh)
- end # MC
- RMSE[Ni,Ti] = E/montecarlo_runs
- propagated_particles += montecarlo_runs*N*T
- # figure();plot([x xh])
-
- @show N
- end # N
- @show T
-end # T
-println("Propagated $propagated_particles particles")
-#
-return RMSE
+ xp = Array{Float64}(n,N,T)
+ w = Array{Float64}(n,N,T)
+ x = Array{Float64}(n,T)
+ y = Array{Float64}(p,T)
+
+ E = sum(1:montecarlo_runs) do
+ xh,xhprev,w = init_pf(N, p0)
+
+ u = randn(m,T)
+ x[:,1] = rand(p0)
+ y[:,1] = C*x[:,1] + rand(pg)
+ error = 0.0
+ @inbounds for t = 1:T-1
+ x[:,t+1] = f(x[:,t],u[:,T]) + rand(pf)
+ y[:,t+1] = C*x[:,t+1] + rand(pg)
+ pf!(xh, xhprev, w, u[:,t], y[:,t], g, f)
+ error += sum(abs2,x[:,t]-weigthed_mean(xh,w))
+ end # t
+ √(error/T)
+ end # MC
+ RMSE[Ni,Ti] = E/montecarlo_runs
+ propagated_particles += montecarlo_runs*N*T
+ # figure();plot([x xh])
+
+ @show N
+ end # N
+ @show T
+ end # T
+ println("Propagated $propagated_particles particles")
+ #
+ return RMSE
end
@time pf!(eye(4),eye(4), ones(4), 4, g, f, σw )