def stackElements(S1, S2):
# Reursive relations, eq (59-62) in "A combined three-dimensional finite element and scattering matrix
# method for the analysis of plane wave diffraction by bi-periodic, multilayered structures"
# (Dossou 2012a)
RIn1, TIn1, TOut1, ROut1 = StoRT(S1)
RIn2, TIn2, TOut2, ROut2 = StoRT(S2)
I = pl.identity(RIn1.shape[0])
RIn = RIn1 + TOut1 * RIn2 * pl.inv(I - ROut1 * RIn2) * TIn1
TIn = TIn2 * pl.inv(I - ROut1 * RIn2) * TIn1
ROut = ROut2 + TIn2 * ROut1 * pl.inv(I - RIn2 * ROut1) * TOut2
TOut = TOut1 * pl.inv(I - RIn2 * ROut1) * TOut2
return RTtoS(RIn, TIn, TOut, ROut)
def compute_registeration(P0, Yk):
'''
compute the registeration. which is transformation martix
ref: http://www.cs.virginia.edu/~mjh7v/bib/Besl92.pdf
Input:
P0: Point set
Yk: P0 corresponding Point set
Output:
qk: transform matrix
'''
# compute mu p and mu x
mup = np.mean(P0, axis=0)
Nx = len(P0)
mux = np.mean(Yk, axis=0)
# Cross-covariance martix sigma_px
sigma_px = (np.dot(P0.T,Yk) - np.dot(mup.T,mux))/Nx
Aij = sigma_px - sigma_px.T
tr = trace(sigma_px)
Sym = sigma_px + sigma_px.T - tr*identity(3)
Q_sigma_px = np.array([
[tr, Aij[1][2], Aij[2][0], Aij[0][1]],
[Aij[1][2], Sym[0][0], 0, 0],
[Aij[2][0], 0, Sym[1][1], 0],
[Aij[0][1], 0, 0, Sym[2][2]]])
# eigenvalue to get optimal R and T
w, v = np.linalg.eig(Q_sigma_px)
qR = v[:, argmin(w)]
R = get_rotation_matrix(qR)
qT = (mux.T - R.dot(mup.T)).T
return qR, qT
# inputs
un = array([0]) # control vector
zn = [] # measurement vector
# outputs
xn = array([3]) # newest estimate of true state
pn = array([1]) # newest estimate of average error
# constants
A = array ([1]) # state transition matrix, used to convert last time's state to newest state
B = array([0]) # control matrix
H = array([1]) # observation matrix, used to convert measurement to state
Q = array([0.00001]) # estimated process error covariance
R = array([0.1]) # estimated measurement error covariance
I = identity(1)
# A is 1 as the state is constant
# H is 1 as the measurement is already in voltage
measure_count = 500
measurements = []
estimations = []
for i in range(measure_count):
zn = 0.75 + random.random ()
xn = A*xn + B*un
pn = A * pn * A.transpose () + Q
y = zn - H * xn
S = H * pn * H.transpose () + R
K = pn * H.transpose () * (1/S)
# Apply the acceleration to velocity.
self.velocity = map(self.add, self.velocity, sliced_acceleration)
sliced_velocity = map(self.mult, self.velocity, timeslicevec )
# Apply the velocity to location.
self.loc = map(self.add, self.loc, sliced_velocity)
# Cannonballs shouldn't go into the ground.
if self.loc[1] < 0:
self.loc[1] = 0
# constants
g = -9.81
dt = 0.1
A = matrix ([[1, dt, 0, 0], [0,1,0,0], [0,0,1,dt], [0,0,0,1]]) # state transition matrix, used to convert last time's state to newest state
B = matrix([[0,0,0,0], [0,0,0,0], [0,0,1,0], [0,0,0,1]]) # control matrix
H = identity (4) # observation matrix, used to convert measurement to state
Q = zeros ((4,4)) # estimated process error covariance
R = identity(4) * 0.2 # estimated measurement error covariance
I = identity (4)
# inputs
un = matrix([[0], [0], [0.5 * g * dt * dt], [g * dt]]) # control vector
zn = [] # measurement vector
# outputs
xn = matrix([[0], [100 * math.cos (math.pi/4)], [500], [100 * math.sin (math.pi / 4)]]) # newest estimate of true state
pn = identity (4) # newest estimate of average error
cannon_ball = CannonBall (dt, 30)
measure_count = 144
measurements = []
def main():
# do we want an image of the loops in 3D for different A?
loops = False
# do we want to make a movie of the stability multipliers in the complex plane?
mk_stab_mov = True
if loops:
fig3d = pl.figure
d3ax = fig.add_subplot(111,projection='3d')
#os.mkdir('LoopImgs')
# make a directory for the stability multiplier images --> this will be a movie as a function of
# A
if mk_stab_mov: os.mkdir('StabMovie')
# this variable just exsits so we dont print the A value of the bifurcation point more than
# once.
found_bif = False
# make file to store q (periodicity)
q_file = open("qdata.txt","w")
# make file to store stability multipliers
eig_file = open("data.txt","w")
eig_file.write("eig1 eig2 A\n")
dt = .001
# total number of iterations to perform
# totIter = 10000000
totIter = 50000
totTime = totIter*dt
time = pl.arange(0.0,totTime,dt)
beta = .6
qq = 1.0
# how many cells is till periodicity use x = n*pi/k (n must be even #) modNum = 2*pl.pi/k
modNum = 2.0*pl.pi
# initial conditions for p1 and p2
p1_init_x = pl.pi
p1_init_vx = 0.0
p2_init_x = 0.0
p2_init_vx = 0.0
A_start = 0.2
A = A_start
A_max = .8
A_step = .01
count = 0
# make arrays to keep eigen values. There willl be four eigen values for N=2 so lets hve two seperate
# arrays for them
eigs1 = pl.array([])
eigs2 = pl.array([])
eigs3 = pl.array([])
eigs4 = pl.array([])
# file to write final poition of particle to
final = open("final_position.txt","w")
final.write("Last position of orbit, A\n")
x0 = pl.array([p1_init_vx,p2_init_vx,p1_init_x,p2_init_x])
previous_q = 0.0
while A < A_max:
# initial conditions vector
# set up: [xdot,ydot,x,y]
apx = Two_Particle_Sin1D(A,beta,qq,dt)
sol = odeint(apx.f,x0,time)
print("x0")
print(x0)
#sol[:,2]=sol[:,2]%(2*pl.pi)
# find a single loop of the limit cycle. Might be periodoc over more than one cycle
# returns the solution of just that loop AND the periodicity of the loop
# takes a threshhold number. If it cant find a solution where the begining and end of the
# trajectroy lye within this threshold value than it quits and prints an error
#thresh is distance in the phase place
#thresh = .01
thresh = .00005
# change this back for bifrucation other than FIRST PI BIF
#loop = find_one_full_closed(sol,thresh,dt)
loop = pl.zeros([int(2.0*pl.pi/dt),4])
loop[:,2]+=pl.pi
# the other particle needs to be at zero. Already is
if "stop" in loop:
break
loop_t = pl.arange(0.0,(len(loop))*dt,dt)
if loops :
d3ax.plot(pl.zeros(len(loop))+A,loop[:,2],loop[:,0],color="Black")
#fig = pl.figure()
#ax = fig.add_subplot(111)
##ax.scatter([0.0,pl.pi,2.0*pl.pi],[0.0,0.0,0.0],color="Red")
##ax.plot(loop[:,2],loop[:,0],":",color="Black")
#ax.plot(loop[:,2],loop[:,0],color="Black")
#ax.set_xlabel("$x_1$",fontsize=25)
#ax.set_ylabel("$x_2$",fontsize=25)
##ax.set_xlim([pl.pi-pl.pi/3.0,pl.pi+pl.pi/3.0])
##ax.set_ylim([-.3,.3])
#fig.tight_layout()
#fig.savefig("LoopImgs/"+str(A)+".png",dpi = 300,transparent=True)
##os.system("open LoopImgs/" +str(A)+".png")
#pl.close(fig)
apx.set_sol(loop)
# solution matrix at t=0 is identity matrix -> we need it in 1d arrar form for odeint though
# -> reshape takes care of the the form
w0 = pl.identity(4).reshape(-1)
w_of_t = odeint(apx.mw,w0,loop_t,hmax=dt,hmin=dt)
#w_of_t = odeint(apx.mw,w0,loop_t)
current_q = loop_t[-1]/(2.0*pl.pi)
# print the period of the orbit we are working on
print("q: " + str(current_q))
q_file.write(str(loop_t[-1]/(2.0*pl.pi))+" "+str(A)+"\n")
if current_q > (previous_q+1.0):
print("bifurcation point. A = " +str(A))
previous_q=current_q
# make the matrix form of w_of_t
matrix = w_of_t[-1,:].reshape(4,4)
print('solution matrix is:')
for i in range(len(matrix)):
print(str(matrix[i,:]))
# print('solution matrix times the initial conditions vector:')
# This is wrong because the x0 needs to be re-ordered to b e consistant with the way the
# Jacobian is set up.
# print(pl.dot(matrix,x0))
# use linalg to get the eigen values of the W(t=q) where q is the period time of the orbit
vals,vect = numpy.linalg.eig(matrix)
print('the trace is' + str(matrix.trace()))
print('determinant is: '+ str(pl.det(matrix)))
if((abs(vals[0])<=1.0) and (not found_bif)):
print("this is the bifurcation point (l1)")
print(A)
found_bif = True
if(abs(vals[1])<=1.0 and (not found_bif)):
print("this is the bifurcation point (l2)")
print(A)
found_bif = True
print('number of eigen values is: ' + str(len(vals)))
eigs1 = pl.append(eigs1,vals[0])
eigs2 = pl.append(eigs2,vals[1])
eigs3 = pl.append(eigs3,vals[2])
eigs4 = pl.append(eigs4,vals[3])
eig_file.write(str(vals[0])+" "+str(vals[1])+" "+str(vals[2])+" "+str(vals[3])+" "+str(A)+"\n")
count+=1
x0 = loop[-1,:]
final.write(str(x0)[1:-1]+" "+str(A) +"\n")
A += A_step
print("A: "+str(A))
theta = pl.arange(0,10,.05)
A_arr = pl.arange(A_start,A,A_step)
print('we are above')
while len(A_arr)>len([k.real for k in eigs1]):
A_arr = A_arr[:-1]
while len(A_arr)<len([k.real for k in eigs1]):
A_arr = pl.append(A_arr,A_arr[-1]+A_step)
print('we are below')
fig1 = pl.figure()
ax1 = fig1.add_subplot(111)
ax1.plot(pl.cos(theta),pl.sin(theta))
ax1.plot([k.real for k in eigs1],[l.imag for l in eigs1])
ax1.set_xlabel("Re[$\lambda_1$]",fontsize=25)
ax1.set_ylabel("Im[$\lambda_1$]",fontsize=25)
fig1.tight_layout()
fig1.savefig("eig1.png")
os.system("open eig1.png")
fig2 = pl.figure()
ax2 = fig2.add_subplot(111)
ax2.plot(pl.cos(theta),pl.sin(theta))
ax2.plot([k.real for k in eigs2],[l.imag for l in eigs2])
ax2.set_xlabel("Re[$\lambda_2$]",fontsize=25)
ax2.set_ylabel("Im[$\lambda_2$]",fontsize=25)
fig2.tight_layout()
fig2.savefig("eig2.png")
os.system("open eig2.png")
fig3, ax3 = pl.subplots(2,sharex=True)
ax3[0].plot(A_arr,[k.real for k in eigs1],color='k')
ax3[1].plot(A_arr,[k.imag for k in eigs1],color='k')
ax3[0].set_ylabel("Re[$\lambda_1$]",fontsize = 25)
ax3[1].set_ylabel("Im[$\lambda_1$]",fontsize = 25)
ax3[1].set_xlabel("$A$",fontsize = 25)
fig3.tight_layout()
fig3.savefig("A_vs_eig1.png")
os.system("open A_vs_eig1.png")
fig4, ax4 = pl.subplots(2,sharex=True)
ax4[0].plot(A_arr,[k.real for k in eigs2], color = 'k')
ax4[1].plot(A_arr,[k.imag for k in eigs2], color = 'k')
ax4[0].set_ylabel("Re[$\lambda_2$]",fontsize = 25)
ax4[1].set_ylabel("Im[$\lambda_2$]",fontsize = 25)
ax4[1].set_xlabel("$A$",fontsize = 25)
fig4.tight_layout()
fig4.savefig("A_vs_eig2.png")
os.system("open A_vs_eig2.png")
eig_file.close()
final.close()
## make text file with all extra information
#outFile = open("info.dat","w")
#outFile.write("Info \n coefficient: " + str(coef) \
# + "\nwave number: " +str(k)\
# + "\nomega: " + str(w)\
# + "\ndamping: " + str(damp)\
# + "\ng: " + str(g)\
# + "\ntime step: " + str(dt)\
# + "\ntotal time: " + str(dt*totIter)\
# + "\ntotal iterations: " + str(totIter)\
# + "\nInitial Conditions: \n" +
# "initial x: " +str(initx) \
# +"\ninitial y: " +str(inity) \
# +"\ninitial vx: " +str(initvx)\
# +"\ninitial vy: " +str(initvy) )
#outFile.close()
# line for stable static point
if loops:
line = pl.arange(start_A-.01,start_A,A_step)
pi_line = pl.zeros(len(line))+pl.pi
z_line = pl.zeros(len(line))
d3ax.plot(line,pi_line,z_line,color="Black")
d3ax.set_xlabel("$A$",fontsize=25)
d3ax.set_ylabel("$x_1$",fontsize=25)
d3ax.set_zlabel("$x_2$",fontsize=25)
d3fig.tight_layout()
d3fig.savefig("loops.png",dpi=300)
if mk_stab_mov:
for i in range(len(eigs1)):
s_fig = pl.figure()
s_ax = s_fig.add_subplot(111)
s_ax.plot(pl.cos(theta),pl.sin(theta))
s_ax.scatter(eigs1[i].real,eigs1[i].imag,c='r',s=20)
s_ax.scatter(eigs2[i].real,eigs2[i].imag,c='b',s=20)
s_ax.scatter(eigs3[i].real,eigs3[i].imag,c='b',s=20)
s_ax.scatter(eigs4[i].real,eigs4[i].imag,c='b',s=20)
s_ax.set_xlabel("Re[$\lambda$]",fontsize=25)
s_ax.set_ylabel("Im[$\lambda$]",fontsize=25)
s_fig.tight_layout()
s_fig.savefig("StabMovie/%(num)0.5d_stbl.png"%{"num":i})
pl.close(s_fig)
# Apply the velocity to location.
self.loc = map(self.add, self.loc, sliced_velocity)
# Cannonballs shouldn't go into the ground.
if self.loc[1] < 0:
self.loc[1] = 0
# constants
g = -9.81
dt = 0.1
A = matrix(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]
) # state transition matrix, used to convert last time's state to newest state
B = matrix([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0,
1]]) # control matrix
H = identity(4) # observation matrix, used to convert measurement to state
Q = zeros((4, 4)) # estimated process error covariance
R = identity(4) * 0.2 # estimated measurement error covariance
I = identity(4)
# inputs
un = matrix([[0], [0], [0.5 * g * dt * dt], [g * dt]]) # control vector
zn = [] # measurement vector
# outputs
xn = matrix([[0], [100 * math.cos(math.pi / 4)], [500],
[100 * math.sin(math.pi / 4)]]) # newest estimate of true state
pn = identity(4) # newest estimate of average error
cannon_ball = CannonBall(dt, 30)
measure_count = 144
un = array([0]) # control vector
zn = [] # measurement vector
# outputs
xn = array([3]) # newest estimate of true state
pn = array([1]) # newest estimate of average error
# constants
A = array(
[1]
) # state transition matrix, used to convert last time's state to newest state
B = array([0]) # control matrix
H = array([1]) # observation matrix, used to convert measurement to state
Q = array([0.00001]) # estimated process error covariance
R = array([0.1]) # estimated measurement error covariance
I = identity(1)
# A is 1 as the state is constant
# H is 1 as the measurement is already in voltage
measure_count = 500
measurements = []
estimations = []
for i in range(measure_count):
zn = 0.75 + random.random()
xn = A * xn + B * un
pn = A * pn * A.transpose() + Q
y = zn - H * xn
S = H * pn * H.transpose() + R
K = pn * H.transpose() * (1 / S)
def pade(N, lam0, dlam, M, P, x):
N = 2 #The current implementation is only valid for N=2
#diferenctaite all the elemnts of K by k
#Fvec has relation to k_0 through alpha - facter hthat
#if possible separte Fvec out into elements of facters of alpha
#equal to eq 3 in papser, make into coo_matrix for input
A, B = P.interpolate(x)
Mvv_0 = assembleMvv(A, B, M, P)
Mrv_0, Mvr_0, Mtv_0, Mvt_0, fvn_0, frn_0, FFUp_0, FFDown_0 = assembleMxxfx(
M, P)
Mvv_1 = assembledev1Mvv(A, B, M, P)
Mrv_1, Mvr_1, Mtv_1, Mvt_1, fvn_1, frn_1, FFUp_1, FFDown_1 = assembledev1Mxxfx(
M, P, FFUp_0, FFDown_0)
Mvv_2 = assembledev2Mvv(A, B, M, P)
#check fvn. frn
Mrv_2, Mvr_2, Mtv_2, Mvt_2, fvn_2, frn_2 = assembledev2Mxxfx(
M, P, FFUp_0, FFDown_0, FFUp_1, FFDown_1)
# These matrices are just used for checks later on:
I = pl.identity(M.NM) #Identity matrix
Z = pl.zeros((M.NM, M.NM))
P_0 = pl.bmat([[Mvv_0, Mvr_0, Mvt_0], [Mrv_0, M.lx * I, Z],
[Mtv_0, Z, M.lx * I]])
P_1 = pl.bmat([[Mvv_1, Mvr_1, Mvt_1], [Mrv_1, Z, Z], [Mtv_1, Z, Z]])
P_2 = pl.bmat([[Mvv_2, Mvr_2, Mvt_2], [Mrv_2, Z, Z], [Mtv_2, Z, Z]])
#Note that u contains the same content as sol,r and t
u = []
sol = []
r = []
t = []
#Note that the LU factorisation on calcvrt0 can be reused for calculating all the u's!
#(which would speed up things, but not change the result obviously...)
frn_0[M.Nm, 0] = -M.lx #mode 0 incidence
ftn_0 = pl.zeros(frn_0.shape, dtype=complex)
sol_0, r_0, t_0 = calcvrt0(M, Mvv_0, Mrv_0, Mvr_0, Mtv_0, Mvt_0, fvn_0,
frn_0, ftn_0)
sol.append(sol_0)
r.append(r_0)
t.append(t_0)
u.append(pl.concatenate((sol_0, r_0, t_0)))
if 1:
#Check that the solution is correct
f = pl.concatenate((fvn_0, frn_0, ftn_0))
print("Solution error for 0'th order: ", abs(P_0 * u[0] - f).max())
_Mvv = Mvv_0
_Mrv = Mrv_0
_Mvr = Mvr_0
_Mtv = Mtv_0
_Mvt = Mvt_0
_fvn = -(Mvv_1 * sol[0] + Mvr_1 * r[0] + Mvt_1 * t[0])
_frn = -(Mrv_1 * sol[0])
_ftn = -(Mtv_1 * sol[0])
sol_1, r_1, t_1 = calcvrt0(M, _Mvv, _Mrv, _Mvr, _Mtv, _Mvt, _fvn, _frn,
_ftn)
sol.append(sol_1)
r.append(r_1)
t.append(t_1)
u.append(pl.concatenate((sol_1, r_1, t_1)))
if 1:
print("Solution error for 1'th order: ",
abs(P_0 * u[1] + P_1 * u[0]).max())
#This will not work for our implementation since higher order derivates of the matrix is non-zero
#And eqn(19) in Jensen 2007 therefore has to be recalculated for our specific case. This is caused by
#derivations of the small dense transmission/reflection matrices (Mrv,Mvt and so on) being non-zero
if 0:
for i in range(2, N + 1):
sol_i, r_i, t_i = calcvrt0(
M, Mvv_0, Mrv_0, Mvr_0, Mtv_0, Mvt_0,
-(Mvv_1 * sol[i - 1] + Mvr_1 * r[i - 1] + Mvt_1 * t[i - 1]) -
(Mvv_2 * sol[i - 2] + Mvr_2 * r[i - 2] + Mvt_2 * t[i - 2]),
-(Mrv_1 * sol[i - 1]) - (Mrv_2 * sol[i - 2]),
-(Mtv_1 * sol[i - 1]) - (Mtv_2 * sol[i - 2]))
sol.append(sol_i)
r.append(r_i)
t.append(t_i)
u.append(pl.concatenate((sol_i, r_i, t_i)))
sol_N1, r_N1, t_N1 = calcvrt0(
M, Mvv_0, Mrv_0, Mvr_0, Mtv_0, Mvt_0,
-(Mvv_1 * sol[N] + Mvr_1 * r[N] + Mvt_1 * t[N]) -
(Mvv_2 * sol[N - 1] + Mvr_2 * r[N - 1] + Mvt_2 * t[N - 1]),
-(Mrv_1 * sol[N]) - (Mrv_2 * sol[N - 1]),
-(Mtv_1 * sol[N]) - (Mtv_2 * sol[N - 1]))
_fvn = -(Mvv_1 * sol[1] + Mvr_1 * r[1] + Mvt_1 * t[1])
_fvn += -(Mvv_2 * sol[0] + Mvr_2 * r[0] + Mvt_2 * t[0])
_frn = -(Mrv_1 * sol[1])
_frn += -(Mrv_2 * sol[0])
_ftn = -(Mtv_1 * sol[1])
_ftn += -(Mtv_2 * sol[0])
sol_2, r_2, t_2 = calcvrt0(M, _Mvv, _Mrv, _Mvr, _Mtv, _Mvt, _fvn, _frn,
_ftn)
sol.append(sol_2)
r.append(r_2)
t.append(t_2)
u.append(pl.concatenate((sol_2, r_2, t_2)))
if 1:
print("Solution error for 2'th order: ",
abs(P_0 * u[2] + P_1 * u[1] + P_2 * u[0]).max())
uconjT = pl.bmat([vec.conj() for vec in u[::-2]]).T
urev = pl.bmat(u[::-2])
Pmat = uconjT * urev
Q = pl.linalg.inv(Pmat)
#Check the inversion... Even though it succeeds
if 0:
print("Pmax:", abs(Pmat).max())
print("P*Q:")
print(Pmat * Q)
uplus = Q * uconjT
b = -uplus * u[N]
#Check if b solves the equation we wanted
if 1:
print("Error for b:", abs(urev * b + u[N]).max())
a = []
a += [0] #If we start summing at 1, then the 0'th element should be zero
#b starts at index 1 and not zero in Jensen 2007, there the elements have to be shifted one
a += [u[1] + b[0, 0] * u[0]]
if N != 2:
exit("this part only implemented for N=2")
a += [u[2] + b[0, 0] * u[1] + b[1, 0] * u[0]]
#List of wavelengths
llam = pl.linspace(lam0 - dlam, lam0 + dlam, 81)
#List of sigmas with k0 subtracted
lsigma = 2 * pl.pi / llam - 2 * pl.pi / lam0
lR = []
for sigma in lsigma:
u_nom = u[0] + a[1] * sigma + a[2] * sigma**2
u_denom = 1 + b[0, 0] * sigma + b[1, 0] * sigma**2
usigma = u_nom / u_denom
nnodes = (M.nelx + 1) * (M.nelz + 1)
solsigma = usigma[:nnodes]
rsigma = usigma[nnodes:nnodes + M.NM]
tsigma = usigma[nnodes + M.NM:]
mR, thetaR, R, mT, thetaT, T = calcRT(M, P, rsigma, tsigma)
lR += [float(R[mR == 0])]
return llam, lR
def calcScatteringMatrix2(M, P, x):
## FE analysis to acquire scattering matrices (see Dossou2006) instead of solving for one incident
## condition as done in FE().
#
#@param M Model object containing geometrical information
#@param P Physics object containing physical parameters
#@param x The design domain with 0 corresponding to eps1 and 1 corresponding to eps2
# and intermediate values to weighted averages in between. See physics.interpolate
#@return Dictionary containing the matrices R,T,R',T' that the scattering matrix consists of.
# as well as normal data
#
#see also FE()
A, B = P.interpolate(x)
Mvv = assembleMvv(A, B, M, P)
Mrv, Mvr, Mtv, Mvt, fvn, frn = assembleMxxfx(M, P)
lu = splu(Mvv)
#Mhatrr and Mhattr
b = lu.solve(Mvr)
Mhatrr = -Mrv * b
_addToDiagonal(Mhatrr, M.lx)
Mhattr = -Mtv * b
#Mhatrt and Mhattt
b = lu.solve(Mvt)
Mhatrt = -Mrv * b
Mhattt = -Mtv * b
_addToDiagonal(Mhattt, M.lx)
#fhatrn
b = lu.solve(fvn)
a = Mrv * b
fhatrn = frn - a
#fhattn
b = lu.solve(fvn)
a = Mtv * b
fhattn = -a
MAT = pl.bmat([[Mhatrr, Mhatrt], [Mhattr, Mhattt]])
RHS = pl.bmat([[Mhatrr - 2 * M.lx * pl.identity(M.NM), Mhatrt],
[Mhattr, Mhattt - 2 * M.lx * pl.identity(M.NM)]])
#Solve Eq (53) in Dossou 2006
RTtilde = pl.solve(MAT, RHS)
#print("pl.diag(pl.asarray(pl.transpose(RTtilde)).reshape(-1))")
#print(pl.diag(pl.asarray(pl.transpose(RTtilde)).reshape(-1)))
matL = pl.bmat([[pl.diag(pl.sqrt(P.chiIn)),
pl.zeros((M.NM, M.NM))],
[pl.zeros((M.NM, M.NM)),
pl.diag(pl.sqrt(P.chiOut))]])
matR = pl.bmat([[pl.diag(1 / pl.sqrt(P.chiIn)),
pl.zeros((M.NM, M.NM))],
[pl.zeros((M.NM, M.NM)),
pl.diag(1 / pl.sqrt(P.chiOut))]])
V = matL * RTtilde * matR
RIn = V[:M.NM, :M.NM]
TIn = V[M.NM:, :M.NM]
ROut = V[M.NM:, M.NM:]
TOut = V[:M.NM, M.NM:]
results = {}
results["x"] = x
results["RIn"] = RIn
results["TIn"] = TIn
results["ROut"] = ROut
results["TOut"] = TOut
results.update(M.getParameters())
results.update(P.getParameters())
return results