trainRBF_ARX.jl 14.8 KB
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
1
2
using Devectorize
using Clustering
3
4
5
# using Debug
include("levenberg_marquardt.jl")
include("../cuckooSearch.jl")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
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

type RbfNonlinearParameters
    x::Vector{Float64}
    n_state::Integer
    n_centers::Integer
end





# for op = (:+, :*, :\, :/)
#     @eval ($op)(a::RbfNonlinearParameters,b) = ($op)(a.x,b)
#     @eval ($op)(b,a::RbfNonlinearParameters) = ($op)(b,a.x)
# end

Base.display(p::RbfNonlinearParameters) = println("RbfNonlinearParameters: Parameters = $(length(p.x)), n_state(x) = $(p.n_state), n_centers(x) = $(p.n_centers)")
Base.start(p::RbfNonlinearParameters) =  1
Base.done(p::RbfNonlinearParameters, state) = state > p.n_centers
Base.next(p::RbfNonlinearParameters, state) = (p.x[1+(state-1)*2*p.n_state:state*2p.n_state])::VecOrMat, state + 1
"""Train your RBF-ARX network.`trainRBF_ARX(y, na, nc; state = :A, liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, outputnet = true, cuckoosearch = false)`\n
The number of centers is equal to `nc` if Kmeans is used to get initial centers, otherwise the number of centers is `nc^n_state`\n
`n_state` is equal to the state dimension, possibly reduced to `inputpca` if so desired.\n
The number of nonlinear parameters is `n_centers × n_state`\n
The number of linear parameters is `outputnet ? n_state × (n_centers+1) × (na)+1) : (na)×(n_centers+1)+1)`"""
31
function trainRBF_ARX(y, A, state, nc; liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, outputnet = true, cuckoosearch = false, cuckooiter=100)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
32
33
34
35
36
    n_points = length(y)
    na = isa(A,Matrix) ? size(A,2) : 1

    function predictionerror(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
37
38
        psi = getΨ(Ψ, znl, state, n_points, n_state, normalized)
        getLinearRegressor(V,psi,A,state,outputnet,na,n_state,n_centers,n_points)
39
        zl = fitlinear(V,y);
40
        prediction = V*zl
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
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
        error = prediction-y
        return error
    end

    # Get initial centers ================================
    Znl::RbfNonlinearParameters
    if isa(inputpca, Int)
        if inputpca > size(state,2)
            warn("inputpca must be <= n_state")
            inputpca = size(state,2)
        end
        state-= repmat(mean(state,1),n_points,1)
        state./= repmat(var(state,1),n_points,1)
        C,score,latent,W0 = PCA(state,true)
        state = score[:,1:inputpca]
    end
    n_state = size(state,2)

    if lowercase(initialcenters) == "equidistant"
        initialcenters = :equidistant
        n_centers = nc^n_state
    else
        initialcenters = :kmeans
        n_centers = nc
    end

    Ψ = Array(Float64,(n_points,n_centers+1))
    Ψ[:,end] = 1.0
    V = outputnet ? V = Array(Float64,(n_points, n_state* (n_centers+1)* (na)+1)) : V = Array(Float64,(n_points, (na)*(n_centers+1)+1))
    V[:,end] = 1.0
    if initialcenters == :equidistant
        Znl = getcentersEq(state,nc); debug("gotcentersEq")
    else
74
        Znl = getcentersKmeans(state, nc, predictionerror, n_state); debug("gotcentersKmeans")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
75
76
    end
    @ddshow Znl
77
    getΨ(Ψ, Znl, state, n_points, n_state, normalized); debug("Got Ψ")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
78
    @ddshow sum(!isfinite(Ψ))
79
    getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points); debug("Got linear regressor V")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
80
81
    @ddshow size(V)
    @ddshow sum(!isfinite(V))
82
    Zl = fitlinear(V,y); debug("fitlinear")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
83
84
85
86
    @ddshow sum(!isfinite(Zl))
    prediction = V*Zl
    error = y - prediction
    errors = zeros(liniters+1)
87
    Lb,Ub = getbounds(Znl, state, n_state, n_centers)
88

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
89
90
    # ============= Main loop  ================================================
    debug("Calculating initial error")
91
    errors[1] = rms(predictionerror(Znl.x))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
92
    println("Training RBF_ARX Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl))")
93
94
    function g(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
95
96
        w = fitlinear(V,y)
        return outputnet ? jacobian_outputnet(znl,Ψ, w, V) : jacobian_no_outputnet(znl,Ψ, w, V)
97
98
99
    end
    f(z) = predictionerror(z)
    X0 = deepcopy(Znl.x)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
100
101
102
103
104
105
106
    for i = 1:liniters
        if i%2 == 1 || !cuckoosearch
            @time res = levenberg_marquardt(f, g, X0,
                                            maxIter = nonliniters,
                                            tolG = 1e-7,
                                            tolX = 1e-10,
                                            show_trace=true,
107
108
109
                                            timeout = 60,
                                            n_state = n_state)
            X0 = deepcopy(res.minimum)
110
111
            DEBUG && assert(X0 == res.minimum)
            DEBUG && @show ff1 = rms(f(X0))
112
113
114
115
            if DEBUG
                _V = deepcopy(V)
                 = deepcopy(Ψ)
            end
116
117
            DEBUG && @show ff2 = rms(f(res.minimum))

118
            if DEBUG
119
                @assert ff1 == ff2
120
121
122
123
124
125
126
                @show res.minimum == X0
                @show _V == V
                @show  == Ψ
            end
            assert(X0 == res.minimum)
            #             Znl = RbfNonlinearParameters(saturatePrecision(copy(res.minimum),n_state),n_state,n_centers)
            Znl = RbfNonlinearParameters(deepcopy(res.minimum),n_state,n_centers)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
127
128
129
130
            errors[i+1] = res.f_minimum
            # show(res.trace)
        else
            display("Using cuckoo search to escape local minimum")
131
132
            @time (bestnest,fmin) = cuckoo_search(x -> rms(f(x)),X0, Lb, Ub;
                                                  n=50,
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
133
134
                                                  pa=0.25,
                                                  Tol=1.0e-5,
135
                                                  max_iter = i < liniters-1 ? cuckooiter : 2cuckooiter,
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
136
137
                                                  timeout = 120)
            debug("cuckoo_search done")
138
139
140
            X0 = deepcopy(bestnest)
            @ddshow rms(f(X0))
            Znl = RbfNonlinearParameters(deepcopy(bestnest),n_state,n_centers)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
141
142
143
144
145
146
147
148
149
            errors[i+1] = fmin
        end
        if abs(errors[i+1]-errors[i]) < 1e-10
            display("No significant change in function value")
            break
        end
    end

    # Test ===============================================
150
151
    getΨ(Ψ, Znl, state, n_points, n_state, normalized)
    getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points)
152
    Zl = fitlinear(V,y); debug("fitlinear")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
    prediction = V*Zl
    error = y - prediction
    if PYPLOT || WINSTON
        newplot(y,"k");
        plot(prediction,"b");
        plot(error,"r");title("Fitresult, RBF-ARX, n_a: $na, n_c: $(Znl.n_centers), Nonlin params: $(length(Znl.x)), Lin params: $(length(Zl)) RMSE = $(rms(error)) Fit = $(fit(y,prediction))")
        plotcenters(Znl)
        newplot(errors,"o"); title("Errors");
    end

    # Exit ===============================================
    println("tainRBF_ARX done. Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl)), RMSE: $(rms(error))")



end



function getcentersEq(state::VecOrMat, nc::Integer)
    n_points = size(state,1)
    n_state = typeof(state) <: Matrix ? size(state,2) : 1
    state_extrema = [minimum(state,1)' maximum(state,1)']
    statewidths = state_extrema[:,2] - state_extrema[:,1]
    Δc = statewidths/nc
    Z = zeros(nc, 2*n_state) # 2*n_state to fit center coordinates and scaling parameters
    # Fill initial centers
    for i = 1:n_state
        Z[:,i] = collect((state_extrema[i,1]+ Δc[i]/2):Δc[i]:state_extrema[i,2])
    end
    # add bandwidth parameters γ, give all centers the same bandwidth with Δc as a (hopefully) good initial guess
    #     display(Z)
    Z[:,n_state+1:end] = 1*repmat(4./(Δc.^2)',nc,1) # Spread the initial guess to all centers
    assert(all(Z[:,n_state+1:end].> 0))
    debug("Z done")
    n_centers::Int64 = nc^n_state # new number of centers wich considers gridding of 1D centers
    ZZ1 = zeros(n_state, n_centers)
    ZZ2 = zeros(n_state, n_centers)
    # Here comes the magic. Spread each one dimensional center onto a grid in n_state dimensions
    v = n_centers
    h = 1
    ii = 1
    for i = 1:n_state # For each iteration, v decreases and h increases by a factor of nc. If v and h are used in repmat(⋅,v,h) which is then vecotrized, the desired grid will be created. It's a bit tricky, but makes sense after a lot of thinking
        v = convert(Int64, v / nc)
        ZZ1[i,:] = vec(repmat(Z[:,i]',v,h))'
        ZZ2[i,:] = vec(repmat(Z[:,i+n_state]',v,h))'
        h *= nc
        ii += 1
    end
    debug("ZZ done")
    RbfNonlinearParameters(vec([ZZ1; ZZ2]), n_state, n_centers)
    #error("Bias parameter!")
end




210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
function getΨ(Ψ, Znl, state, n_points, n_state, normalized::Bool)
    Ψ = normalized ? getΨnormalized(Ψ, Znl, state, n_points, n_state) :  getΨnonnormalized(Ψ, Znl, state, n_points, n_state)
    if DEBUG && sum(!isfinite(Ψ)) > 0
        @show sum(!isfinite(Ψ))
    end
    return Ψ
end

function getΨnormalized(Ψ, Znl, state, n_points, n_state)
    RBF(x, Znl::VecOrMat,n_state::Integer) = exp(-(((x-Znl[1:n_state]).^2.*Znl[n_state+1:end])[1]))
    rowsum = ones(n_points)
    for (j,Zin) in enumerate(Znl)
        Zi = deepcopy(Zin)
        #         for i = n_state+1:2n_state
        #             Zi[i] = Zi[i] <= 0 ? 0.01 : Zi[i] # Reset to 1 if precision became negative
        #         end
        for i = 1:n_points
            statei = squeeze(state[i,:]',2)
            a = RBF(statei, Zi, n_state)
            Ψ[i,j] = a
            rowsum[i] += a
        end
    end
    for i = 1:n_points
        if rowsum[i] <= 1e-10
            continue
        end
        @devec Ψ[i,:] ./= rowsum[i]
    end
    return Ψ
end

function getΨnonnormalized(Ψ, Znl, state, n_points, n_state)
    RBF(x, Znl::VecOrMat,n_state::Integer) = exp(-(((x-Znl[1:n_state]).^2.*Znl[n_state+1:end])[1]))
    for (j,Zin) in enumerate(Znl)
        Zi = deepcopy(Zin)
        for i = 1:n_points
            statei = squeeze(state[i,:]',2)
            #                     statei = slice(state,i,:)
            Ψ[i,j] = RBF(statei, Zi, n_state)
            if DEBUG && !isfinite(Ψ[i,j])
                @show i,j,statei, Zi, n_state, Ψ[i,j]
                @show (statei-Zi[1:n_state]).^2
                @show Zi[n_state+1:end]
                #                     @show exp(-(((statei-Zi[1:n_state]).^2.*Zi[n_state+1:end])[1]))
                error("Stopping")
            end
        end
    end
    return Ψ
end

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
262

263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
function getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points)
    if outputnet
        ii = 1
        for i = 1:na
            for k = 1:n_centers+1
                for j = 1:n_state
                    for l = 1:n_points
                        V[l,ii] = Ψ[l,k]*A[l,i]*state[l,j]
                    end
                    ii = ii+1
                end
            end
        end
    else
        ii = 1
        for i = 1:na
            for k = 1:n_centers+1
                for l = 1:n_points
                    V[l,ii] = Ψ[l,k]*A[l,i]
                end
                ii = ii+1
            end
        end
    end
    if DEBUG && sum(!isfinite(V)) > 0
        @show sum(!isfinite(V))
    end
    return V
end
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336


function plotcenters(Z)
    X = zeros(Z.n_centers,2)
    for (i,Zi) in enumerate(Z)
        X[i,:] = Zi[1:2]'
    end
    newplot(X[:,1],X[:,2],"o"); title("Centers")
end

function getbounds(Znl, state, n_state, n_centers)
    Lb = zeros(Znl.x)
    Ub = zeros(Znl.x)
    mas = maximum(state,1)'
    mis = minimum(state,1)'
    for i = 1:2n_state:n_centers*2n_state
        Lb[i:i+n_state-1] = mis
        Ub[i:i+n_state-1] = mas
        Lb[i+n_state:i+2n_state-1] = 0.000001
        Ub[i+n_state:i+2n_state-1] = 10*Znl.x[n_state+1:2n_state]
    end
    return Lb,Ub
end


function getcentersKmeans(state, nc::Int, f::Function, n_state::Int)
    iters = 21
    errorvec = zeros(iters)
    params = Array(Float64,(nc*2*n_state,iters))
    methods = [:rand;:kmpp]
    for iter = 1:iters
        clusterresult = kmeans(state', nc; maxiter=200, display=:none, init=iter<iters ? methods[iter%2+1] : :kmcen)
        for i = 1:nc
            si = 1+(i-1)n_state*2
            params[si:si+n_state-1,iter] = clusterresult.centers[:,i]
            C = cov(state[clusterresult.assignments .== i,:])
            params[si+n_state:si+2n_state-1,iter] = diag(inv(C))
            @assert !any(diag(inv(C)) .< 0)
        end
        errorvec[iter] = rms(f(params[:,iter]))
    end
    println("Std in errors among initial centers: ", round(std(errorvec),3))
    ind = indmin(errorvec)
    return RbfNonlinearParameters(params[:,ind],n_state, nc)
end
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414


function jacobian_outputnet(Znl, Ψ, w, V)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    J = Array(Float64,(n_points,length(Znl.x)))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = 1:n_points
            Ψw = 1.0
            for i = 1:na
                for j = 1:n_state
                    ind = j + n_state*(k-1) + n_state*(n_centers+1)*(i-1)
                    Ψw  += V[l,ind]*w[ind]
                end
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[l,i1+p] = 2*Ψw*x_μ*γ[p]
                J[l,i1+n_state+p] = (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    return J
end

function jacobian_no_outputnet(Znl, Ψ, w,v)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    J = Array(Float64,(n_points,length(Znl.x)))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = 1:n_points
            Ψw = 1.0
            for i = 1:na
                ind = k + (n_centers+1)*(i-1)
                Ψw  += V[l,ind]*w[ind]
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[l,i1+p] = 2*Ψw*x_μ*γ[p]
                J[l,i1+n_state+p] = (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    return J
end

function saturatePrecision(x,n_state)
    for i = 1:2n_state:length(x)
        range = i+n_state:i+2n_state-1
        x[range] = abs(x[range])
    end
    return x
end

function fitlinear(V,y)
    try
        DEBUG && assert(isa(V,Matrix))
        DEBUG && assert(isa(y,Vector))
        DEBUG && assert(!any(!isfinite(V)))
        DEBUG && assert(!any(!isfinite(y)))
        return V\y
    catch ex
        display(ex)
        error("Linear fitting failed")
    end
end