# QVA with local Hadamard Overlap

I am trying to implement in Qiskit the quantum variational linear solver given here (https://arxiv.org/pdf/1909.05820.pdf) with a local Hadamard overlap test and failing substantially. The cost is not correct and changes very little iteratively. I'm fairly certain my issue in the test routine itself. Any help would be greatly appreciated in figuring out what is going wrong here.
Best, -Corey

# QVA CODE
n_qubits = 3  # Number of system qubits
nlayers = 2
maxiter = 200
tot_qubits = n_qubits + 1  # Addition of an ancillary qubit.
ancilla_idx = n_qubits  # Index of the ancillary qubit (last position).

# Coefficients of the linear combination A = c_0 A_0 + c_1 A_1 ...
c = np.array([1.0, 0.2, 0.2])
g = [[0, 0, 0],[1, 3, 0],[1,0,0]]

def U_b(circ):
for idx in range(n_qubits):
circ.h(idx)
return circ

def CA(idx, circ):
assert(len(g[idx])==3)
for iqbit in range (0, len(g[idx])): # loop over the qubits in the gate
if (g[idx][iqbit] == 0):
None # Identity Operation
elif (g[idx][iqbit] == 1):
circ.cx(ancilla_idx, iqbit)
elif (g[idx][iqbit] == 2):
circ.cy(ancilla_idx, iqbit)
elif (g[idx][iqbit] == 3):
circ.cz(ancilla_idx, iqbit)
else:
raise Error("code should not be here")
return circ

# Variational circuit mapping the ground state |0> to the ansatz state |x>
def variational_block(parameters, circ):
parameter_count = 0
for idx in range (0, n_qubits):
circ.ry(parameters[parameter_count], idx)
parameter_count += 1

for ilayer in range(1,nlayers):
parameter_count, circ = ansatz_layer_RYZ(parameters, parameter_count, circ)

return circ

# Ansantz from: Variational Quantum Linear Solver, Carlos Bravo-Prieto (2020)
def ansatz_layer_RYZ(parameters, parameter_count, circ):
for idx in range (0, n_qubits-1,2):
circ.cz(idx,idx+1)

for idx in range (0, n_qubits):
circ.ry(parameters[parameter_count],idx)
parameter_count += 1

for idx in range (1, n_qubits-1,2):
circ.cz(idx, idx+1)

for idx in range (1, n_qubits-1):
circ.ry(parameters[parameter_count],idx)
parameter_count += 1

return parameter_count, circ

def local_hadamard_test(weights, l=None, lp=None, j=None, part=None):

qctl = QuantumRegister(tot_qubits)
qc   = ClassicalRegister(tot_qubits)
circ = QuantumCircuit(qctl, qc)
backend = Aer.get_backend('aer_simulator')

assert(ancilla_idx == tot_qubits-1)

# First Hadamard gate applied to the ancillary qubit.
circ.h(ancilla_idx)

# For estimating the imaginary part of the coefficient "mu", we must add a "-i"
# phase gate.
if part == "Im" or part == "im":
circ.sdg(ancilla_idx)

# Variational circuit generating a guess for the solution vector |x>
circ = variational_block(weights, circ)

# Controlled application of the unitary component A_l of the problem matrix A.
circ = CA(l,circ)

# Adjoint of the unitary U_b associated to the problem vector |b>.
# In this specific example Adjoint(U_b) = U_b.
circ = U_b(circ)

# Controlled Z operator at position j. If j = -1, apply the identity.
if j != -1:
circ.cz(ancilla_idx, j)

# Unitary U_b associated to the problem vector |b>.
circ = U_b(circ)

# Controlled application of Adjoint(A_lp).
# In this specific example Adjoint(A_lp) = A_lp.
circ = CA(lp, circ)

# Second Hadamard gate applied to the ancillary qubit.
circ.h(ancilla_idx)

circ.save_statevector()
t_circ = transpile(circ, backend)
qobj = assemble(t_circ)
job = backend.run(qobj)
result = job.result()
outputstate = result.get_statevector(circ, decimals=100)
o = outputstate
m_sum = 0
for l in range (0, len(o)):
if (l%2 == 1):
n = o[l] * o[l].conjugate()
m_sum+=n

result = (1-(2*m_sum))

# Expectation value of Z for the ancillary qubit.
return result

def mu(weights, l=None, lp=None, j=None):
mu_real = local_hadamard_test(weights, l=l, lp=lp, j=j, part="Re")
mu_imag = local_hadamard_test(weights, l=l, lp=lp, j=j, part="Im")
return mu_real + 1.0j * mu_imag

def psi_norm(weights):
norm = 0.0
for l in range(0, len(c)):
for lp in range(0, len(c)):
norm = norm + c[l] * np.conj(c[lp]) * mu(weights, l, lp, -1)
return abs(norm)

def cost_loc(weights):
mu_sum = 0.0
for l in range(0, len(c)):
for lp in range(0, len(c)):
for j in range(0, n_qubits):
mu_sum = mu_sum + c[l] * np.conj(c[lp]) * mu(weights, l, lp, j)
mu_sum = abs(mu_sum)
result =  0.5 - 0.5 * mu_sum / (n_qubits * psi_norm(weights))
global cost_values
global nit
cost_values.append(result)
nit = nit + 1;
print("iteration: ",nit," || cost: ",result)
return result

nit = 0
cost_values = []
nparameters = n_qubits + 2*(n_qubits-1)*(nlayers-1)
parameters = [float(random.randint(-3000,3000))/1000 for i in range(0, nparameters)]
out = minimize(cost_loc, parameters, method="COBYLA", options={'maxiter':maxiter})


for l in range (0, len(o)):