trainRBF_ARX.jl 13.1 KB
Newer Older
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
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
77
78
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
122
123
124
125
126
127
128
129
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
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
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
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
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
362
363
364
using Devectorize
using Clustering
using Debug


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

type RbfLinearParameters
    x::Vector{Float64}
end

type RbfParameters
    n::RbfNonlinearParameters
    l::RbfLinearParameters
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))
            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 getΨ(Znl)
        RBF(x, Znl::VecOrMat,n_state::Integer) = exp(-(((x-Znl[1:n_state]).^2.*Znl[n_state+1:end])[1]))
        if normalized
            rowsum = ones(n_points)
            for (j,Zi) in enumerate(Znl)
                for i = n_state+1:2n_state
                    Zi[i] = Zi[i] <= 0 ? 1.0 : 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
        else # Not normalized
            for (j,Zi) in enumerate(Znl)
                for i = n_state+1:2n_state
                    Zi[i] = Zi[i] <= 0 ? 1.0 : Zi[i] # Reset to 1 if precision became negative
                end

                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
        end
        if DEBUG && sum(!isfinite(Ψ)) > 0
            @show sum(!isfinite(Ψ))
        end
        return Ψ
    end

    function fitlinear(V)
        try
            assert(isa(V,Matrix))
            assert(isa(y,Vector))
            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

    function getLinearRegressor(Ψ)
        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

    function predictionerror(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        getΨ(znl);
        getLinearRegressor(Ψ);
        zl = fitlinear(V);
        prediction = V*zl
        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
    getΨ(Znl); debug("Got Ψ")
    @ddshow sum(!isfinite(Ψ))
    getLinearRegressor(Ψ); debug("Got linear regressor V")
    @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)

    # ============= Main loop  ================================================
    debug("Calculating initial error")
    errors[1] = sse(predictionerror(Znl.x))
    println("Training RBF_ARX Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl))")
    for i = 1:liniters
        function g(z)
            znl = RbfNonlinearParameters(z,n_state,n_centers)
            w = fitlinear(V)
            jacobian(znl,Ψ, w)
        end
        f(z) = predictionerror(z)

        X0 = Znl.x

        if i%2 == 1 || !cuckoosearch
            @time res = levenberg_marquardt(f, g, X0,
                                            maxIter = nonliniters,
                                            tolG = 1e-7,
                                            tolX = 1e-10,
                                            show_trace=true,
                                            timeout = 60)
            Znl = RbfNonlinearParameters(saturatePrecision(res.minimum,n_state),n_state,n_centers)
            errors[i+1] = res.f_minimum
            # show(res.trace)
        else
            display("Using cuckoo search to escape local minimum")
            @time (bestnest,fmin) = cuckoo_search(x -> sum(f(x).^2),X0;
                                                  n=30,
                                                  pa=0.25,
                                                  Tol=1.0e-5,
                                                  max_iter = i < liniters-1 ? 80 : 200,
                                                  timeout = 120)
            debug("cuckoo_search done")
            Znl = RbfNonlinearParameters(bestnest,n_state,n_centers)
            errors[i+1] = fmin
        end
        if abs(errors[i+1]-errors[i]) < 1e-10
            display("No significant change in function value")
            break
        end
        getΨ(Znl)
        getLinearRegressor(Ψ)
        fitlinear(V)
        #         Znl.x = res.minimum
    end

    # Test ===============================================
    getΨ(Znl)
    getLinearRegressor(Ψ)
    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