extended_lyapunov.jl 5.52 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
using LinearAlgebra
using FastLapackInterface
using FastLapackInterface.LinSolveAlgo
using FastLapackInterface.SchurAlgo

struct LyapdWs
    AA::Matrix{Float64}
    AAtemp::Matrix{Float64}
    AA2::Matrix{Float64}
    BB::Matrix{Float64}
    temp1::Matrix{Float64}
    XX::Vector{Float64}
MichelJuillard's avatar
MichelJuillard committed
13
    nonstationary_variables::Vector{Bool}
14
    nonstationary_trends::Vector{Bool}
15
16
17
    dgees_ws::DgeesWs
    linsolve_ws1::LinSolveWs
    linsolve_ws2::LinSolveWs
18
    stationary_model::Bool
19
20
21
22
23
24
25
    function LyapdWs(n)
        AA = Matrix{Float64}(undef, n, n)
        AAtemp = Matrix{Float64}(undef, n, n)
        AA2 = Matrix{Float64}(undef, 2*n, 2*n)
        BB = similar(AA)
        temp1 = Matrix{Float64}(undef, n, n)
        XX = Vector{Float64}(undef, 2*n)
MichelJuillard's avatar
MichelJuillard committed
26
        nonstationary_variables = Vector{Bool}(undef, n)
27
        nonstationary_trends = Vector{Bool}(undef, n)
28
29
30
        dgees_ws = DgeesWs(n)
        linsolve_ws1 = LinSolveWs(n)
        linsolve_ws2 = LinSolveWs(2*n)
31
        stationary_model = true
MichelJuillard's avatar
MichelJuillard committed
32
        new(AA, AAtemp, AA2, BB, temp1, XX, nonstationary_variables,
33
            nonstationary_trends, dgees_ws, linsolve_ws1,
34
            linsolve_ws2, stationary_model)
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
    end
end

function solve_one_row!(X, A, B, n, row, ws)
    α = A[row, row]
    vA = view(A, 1:row, 1:row)
    vAA = view(ws.AAtemp, 1:row, 1:row)
    copy!(vAA, vA)
    lmul!(α, vAA)
    vAA .-= I(row)
    #exploiting symmetry to have row vectors
    vX = view(X, 1:row, row)
    vB = view(B, 1:row, row)
    vX .= .-vB
    linsolve_core!(vAA, vX, ws.linsolve_ws1)
    i = row
    j = n*(row - 1) + 1
    while i < row*n
        X[i] = X[j]
        i += n
        j += 1
    end
end

function solve_two_rows!(X, A, B, n, row, ws)
    α11, α21, α12, α22 = A[(row - 1):row, (row - 1):row]
    l1 = 1
    l2 = 1
    while l1 <= row
        k1 = 1
        k2 = 1
        while k1 <= row
            ws.AA2[k2, l2] = α11*A[k1, l1]
            ws.AA2[k2  + 1, l2] = α21*A[k1, l1]
            ws.AA2[k2, l2 + 1] = α12*A[k1, l1]
            ws.AA2[k2 + 1, l2 + 1] = α22*A[k1, l1]
            k1 += 1
            k2 += 2
        end
        ws.XX[l2] = -B[row - 1, l1]
        ws.XX[l2 + 1] = -B[row, l1]
        l1 += 1
        l2 += 2
    end
    vAA = view(ws.AA2, 1:2*row, 1:2*row)           
    vAA .-= I(2*row)
    vXX = view(ws.XX, 1:2*row)
    linsolve_core!(vAA, vXX, ws.linsolve_ws2)
    i = row - 1
    j = n*(row - 2) + 1
    m = 1
    while i <= row*n
        X[i] = vXX[m]
        X[i + 1] = vXX[m+1]
        X[j] = X[i]
        X[j + n] = X[i+1]
        i += n
        j += 1
        m += 2
    end
end

"""
    function lyapd(Σ, A, B, ws) solves equation Σ - A*Σ*A' = B
"""
function extended_lyapd!(Σ, A, B, ws)
    n = size(A, 1)
    copy!(ws.AA, A)
    dgees!(ws.dgees_ws, ws.AA, >, 1-1e-6)
MichelJuillard's avatar
MichelJuillard committed
104
    mul!(ws.temp1, transpose(ws.dgees_ws.vs), B)
105
106
    mul!(ws.BB, ws.temp1, ws.dgees_ws.vs)

MichelJuillard's avatar
MichelJuillard committed
107
108
    extended_lyapd_core!(Σ, ws.AA, ws.BB, ws)
    
109
110
111
    mul!(ws.temp1, ws.dgees_ws.vs, Σ)
    mul!(Σ, ws.temp1, ws.dgees_ws.vs')

112
113
114
115
116
    for i = 1:n
        if ws.nonstationary_trends[i]
            for j = 1:n
                if abs(ws.dgees_ws.vs[j, i]) > 1e-10
                    ws.nonstationary_variables[j] = true
117
                end
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
            end
        end
    end
    
    for i = 1:n
        if  ws.nonstationary_variables[i]
            for j = i:n
                Σ[j, i] = NaN
                Σ[i, j] = NaN                    
            end
        else
            for j = i:n
                if  ws.nonstationary_variables[j]
                    Σ[j, i] = NaN
                    Σ[i, j] = NaN
133
134
135
136
137
138
                end
            end
        end
    end
end

MichelJuillard's avatar
MichelJuillard committed
139
140
function extended_lyapd_core!(Σ, A, B, ws)
    fill!(Σ, 0.0)
141
    fill!(ws.nonstationary_variables, false)
MichelJuillard's avatar
MichelJuillard committed
142
143
144
145
    n = size(A, 1)
    row = n
    while row >= 1
        if row == 1 || A[row, row - 1] == 0
146
147
            if A[row, row] > 1 - 1e-6
                ws.nonstationary_trends[row] = true
148
                ws.stationary_model = false
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
            else 
                solve_one_row!(Σ, A, B, n, row, ws)
                vB = view(B, 1:row - 1, 1:row - 1)
                vtemp = view(ws.temp1, 1:row - 1, 1)
                 = view(Σ, 1:row -1 , row)
                vA = view(A, 1:row - 1, 1:row - 1)
                mul!(vtemp, vA, )
                vA = view(A, 1:row - 1, row)
                mul!(vB, vtemp, vA', 1.0, 1.0)
                 = view(Σ, 1:row, row)
                vA = view(A, 1:row - 1, 1:row)
                mul!(vtemp, vA, )
                vA = view(A, 1:row - 1, row)
                vB .= vB .+ vA.*vtemp'
            end
MichelJuillard's avatar
MichelJuillard committed
164
165
            row -= 1
        else
166
167
168
169
            a = A[row, row]
            if a*a + A[row, row - 1]*A[row - 1, row] > 1 - 2e-6
                ws.nonstationary_trends[row] = true
                ws.nonstationary_trends[row - 1] = true
170
                ws.stationary_model = false
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
            else 
                solve_two_rows!(Σ, A, B, n, row, ws)
                vB = view(B, 1:row - 2, 1:row - 2)
                vtemp = view(ws.temp1, 1:row - 2, 1:2)
                 = view(Σ, 1:row - 2, row - 1:row)
                vA = view(A, 1:row - 2, 1:row - 2)
                mul!(vtemp, vA, )
                vA = view(A, 1:row - 2, row - 1:row)
                mul!(vB, vtemp, vA', 1.0, 1.0)
                 = view(Σ, 1:row, row - 1:row)
                vA = view(A, 1:row - 2, 1:row)
                mul!(vtemp, vA, )
                vA = view(A, 1:row - 2, row - 1:row)
                mul!(vB, vA, vtemp', 1.0, 1.0)
            end
MichelJuillard's avatar
MichelJuillard committed
186
187
188
189
190
            row -= 2
        end
    end
end