trainRBF_ARX.jl 14.3 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
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

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)`"""
function trainRBF_ARX(y, A, state, nc; liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, outputnet = true, cuckoosearch = false)
    n_points = length(y)
    na = isa(A,Matrix) ? size(A,2) : 1

    function getcentersKmeans(state, nc)
        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))
47
                @assert !any(diag(inv(C)) .< 0)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
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
            end
            errorvec[iter] = rms(predictionerror(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


    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 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 fitlinear(V)
        try
77
78
            DEBUG && assert(isa(V,Matrix))
            DEBUG && assert(isa(y,Vector))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
            DEBUG && assert(!any(!isfinite(V)))
            DEBUG && assert(!any(!isfinite(y)))
            return V\y
        catch ex
            @show reshape(Znl.x,2n_state,n_centers)
            display(ex)
            error("Linear fitting failed")
        end
    end

    function jacobian(Znl, Ψ, w)
        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
                if outputnet
                    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
                else
                    for i = 1:na
                        ind = k + (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

122

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
123
124
125

    function predictionerror(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
126
127
128
129
        psi = getΨ(deepcopy(Ψ), znl, state, n_points, n_state, normalized)
        v = getLinearRegressor(deepcopy(V),psi,A,state,outputnet,na,n_state,n_centers,n_points)
        zl = fitlinear(v);
        prediction = v*zl
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
        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
        Znl = getcentersKmeans(state,nc); debug("gotcentersKmeans")
    end
    @ddshow Znl
167
    getΨ(Ψ, Znl, state, n_points, n_state, normalized); debug("Got Ψ")
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
168
    @ddshow sum(!isfinite(Ψ))
169
    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
170
171
172
173
174
175
176
177
    @ddshow size(V)
    @ddshow sum(!isfinite(V))
    Zl = fitlinear(V); debug("fitlinear")
    @ddshow sum(!isfinite(Zl))
    prediction = V*Zl
    error = y - prediction
    errors = zeros(liniters+1)

178
179
180
181
182
183
184
185
186
187
188
189
190
    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
    @show Lb
    @show Ub

Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
191
192
    # ============= Main loop  ================================================
    debug("Calculating initial error")
193
    errors[1] = rms(predictionerror(Znl.x))
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
194
    println("Training RBF_ARX Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl))")
195
196
197
198
199
200
201
    function g(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        w = fitlinear(V)
        jacobian(znl,Ψ, w)
    end
    f(z) = predictionerror(z)
    X0 = deepcopy(Znl.x)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
202
203
204
205
206
207
208
    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,
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
                                            timeout = 60,
                                            n_state = n_state)
            X0 = deepcopy(res.minimum)
            assert(X0 == res.minimum)
            @show rms(f(X0))
            if DEBUG
                _V = deepcopy(V)
                 = deepcopy(Ψ)
            end
            @show rms(f(res.minimum))
            if DEBUG
                @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
227
228
229
230
            errors[i+1] = res.f_minimum
            # show(res.trace)
        else
            display("Using cuckoo search to escape local minimum")
231
232
            @time (bestnest,fmin) = cuckoo_search(x -> rms(f(x)),X0, Lb, Ub;
                                                  n=50,
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
233
234
235
236
237
                                                  pa=0.25,
                                                  Tol=1.0e-5,
                                                  max_iter = i < liniters-1 ? 80 : 200,
                                                  timeout = 120)
            debug("cuckoo_search done")
238
239
240
            X0 = deepcopy(bestnest)
            @ddshow rms(f(X0))
            Znl = RbfNonlinearParameters(deepcopy(bestnest),n_state,n_centers)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
241
242
243
244
245
246
247
248
249
            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 ===============================================
250
251
    getΨ(Ψ, Znl, state, n_points, n_state, normalized)
    getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points)
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
252
253
254
255
256
257
258
259
260
261
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
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
    Zl = fitlinear(V); debug("fitlinear")
    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




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
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
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
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
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