extended_lyapunov.jl 5.52 KB
Newer Older
 MichelJuillard committed May 17, 2020 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 committed Jul 28, 2020 13 `````` nonstationary_variables::Vector{Bool} `````` MichelJuillard committed Aug 23, 2020 14 `````` nonstationary_trends::Vector{Bool} `````` MichelJuillard committed May 17, 2020 15 16 17 `````` dgees_ws::DgeesWs linsolve_ws1::LinSolveWs linsolve_ws2::LinSolveWs `````` MichelJuillard committed Apr 16, 2021 18 `````` stationary_model::Bool `````` MichelJuillard committed May 17, 2020 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 committed Jul 28, 2020 26 `````` nonstationary_variables = Vector{Bool}(undef, n) `````` MichelJuillard committed Aug 23, 2020 27 `````` nonstationary_trends = Vector{Bool}(undef, n) `````` MichelJuillard committed May 17, 2020 28 29 30 `````` dgees_ws = DgeesWs(n) linsolve_ws1 = LinSolveWs(n) linsolve_ws2 = LinSolveWs(2*n) `````` MichelJuillard committed Apr 16, 2021 31 `````` stationary_model = true `````` MichelJuillard committed Jul 28, 2020 32 `````` new(AA, AAtemp, AA2, BB, temp1, XX, nonstationary_variables, `````` MichelJuillard committed Aug 23, 2020 33 `````` nonstationary_trends, dgees_ws, linsolve_ws1, `````` MichelJuillard committed Apr 16, 2021 34 `````` linsolve_ws2, stationary_model) `````` MichelJuillard committed May 17, 2020 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 committed Jul 28, 2020 104 `````` mul!(ws.temp1, transpose(ws.dgees_ws.vs), B) `````` MichelJuillard committed May 17, 2020 105 106 `````` mul!(ws.BB, ws.temp1, ws.dgees_ws.vs) `````` MichelJuillard committed Jul 31, 2020 107 108 `````` extended_lyapd_core!(Σ, ws.AA, ws.BB, ws) `````` MichelJuillard committed May 17, 2020 109 110 111 `````` mul!(ws.temp1, ws.dgees_ws.vs, Σ) mul!(Σ, ws.temp1, ws.dgees_ws.vs') `````` MichelJuillard committed Aug 23, 2020 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 `````` MichelJuillard committed May 17, 2020 117 `````` end `````` MichelJuillard committed Aug 23, 2020 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 `````` MichelJuillard committed May 17, 2020 133 134 135 136 137 138 `````` end end end end end `````` MichelJuillard committed Jul 31, 2020 139 140 ``````function extended_lyapd_core!(Σ, A, B, ws) fill!(Σ, 0.0) `````` MichelJuillard committed Aug 23, 2020 141 `````` fill!(ws.nonstationary_variables, false) `````` MichelJuillard committed Jul 31, 2020 142 143 144 145 `````` n = size(A, 1) row = n while row >= 1 if row == 1 || A[row, row - 1] == 0 `````` MichelJuillard committed Aug 23, 2020 146 147 `````` if A[row, row] > 1 - 1e-6 ws.nonstationary_trends[row] = true `````` MichelJuillard committed Apr 16, 2021 148 `````` ws.stationary_model = false `````` MichelJuillard committed Aug 23, 2020 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) vΣ = view(Σ, 1:row -1 , row) vA = view(A, 1:row - 1, 1:row - 1) mul!(vtemp, vA, vΣ) vA = view(A, 1:row - 1, row) mul!(vB, vtemp, vA', 1.0, 1.0) vΣ = view(Σ, 1:row, row) vA = view(A, 1:row - 1, 1:row) mul!(vtemp, vA, vΣ) vA = view(A, 1:row - 1, row) vB .= vB .+ vA.*vtemp' end `````` MichelJuillard committed Jul 31, 2020 164 165 `````` row -= 1 else `````` MichelJuillard committed Aug 23, 2020 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 `````` MichelJuillard committed Apr 16, 2021 170 `````` ws.stationary_model = false `````` MichelJuillard committed Aug 23, 2020 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) vΣ = view(Σ, 1:row - 2, row - 1:row) vA = view(A, 1:row - 2, 1:row - 2) mul!(vtemp, vA, vΣ) vA = view(A, 1:row - 2, row - 1:row) mul!(vB, vtemp, vA', 1.0, 1.0) vΣ = view(Σ, 1:row, row - 1:row) vA = view(A, 1:row - 2, 1:row) mul!(vtemp, vA, vΣ) vA = view(A, 1:row - 2, row - 1:row) mul!(vB, vA, vtemp', 1.0, 1.0) end `````` MichelJuillard committed Jul 31, 2020 186 187 188 189 190 `````` row -= 2 end end end ``````