def findFeasibleStdFromStd(self, idf, xStd):
# type: (Identification, np._ArrayLike) -> (np._ArrayLike)
''' find closest feasible std solution for some std parameters (increases error) '''
idable_params = sorted(list(set(idf.model.identified_params).difference(self.delete_cols)))
delta = Matrix(idf.model.param_syms[idable_params])
I = Identity
#Pd = Matrix(idf.model.Pd)
#delta_d = (Pd.T*delta)
u = Symbol('u')
U_delta = BlockMatrix([[Matrix([u]), (xStd - delta).T],
[xStd - delta, I(len(idable_params))]])
U_delta = U_delta.as_explicit()
lmis = [LMI_PSD(U_delta)] + self.LMIs_marg
variables = [u] + list(delta)
objective_func = u
prime = idf.model.xStdModel[idable_params]
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime)
u = solution[0, 0]
if u:
print("SDP found std solution with {} error increase from previous solution".format(u))
delta_star = np.matrix(solution[1:])
xStd = np.squeeze(np.asarray(delta_star))
return xStd
def H_representation(V: Iterable[Matrix], S: Iterable[Matrix]):
A_1 = BlockMatrix(tuple(V)).as_explicit()
A_2 = BlockMatrix(tuple(S)).as_explicit()
n = A_1.shape[0]
p = A_1.shape[1]
q = A_2.shape[1]
A_Q = BlockMatrix([
[Identity(n), -A_1, -A_2],
[-Identity(n), A_1, A_2],
[Matrix([n*[0]]), Matrix([p*[1]]), Matrix([q*[0]])],
[ZeroMatrix(p, n), -Identity(p), ZeroMatrix(p, q)],
[ZeroMatrix(q, n), ZeroMatrix(q, p), -Identity(q)]
]).as_explicit()
b_Q = Matrix(2*n*[0] + [1] + p*[0] + q*[0])
return A_Q, b_Q
def __init__(self, arg):
if isinstance(arg, TransferFunctionModel):
# call the private method for realization finding
self.represent = self._find_realization(arg.G, arg.s)
# create a block matrix [[A,B], [C,D]] for visual representation
self.BlockRepresent = BlockMatrix(
[[self.represent[0], self.represent[1]],
[self.represent[2], self.represent[3]]])
return None
else:
# store the argument as representation of the system
try:
self.represent = arg[:4]
except TypeError:
raise TypeError("'repesentation' must be a list-like object")
try:
# assert that A,B,C,D have matching shapes
if not (
(self.represent[0].shape[0] == self.represent[1].shape[0])
and
(self.represent[0].shape[1] == self.represent[2].shape[1])
and
(self.represent[1].shape[1] == self.represent[3].shape[1])
and (self.represent[2].shape[0]
== self.represent[3].shape[0])):
raise ShapeError("Shapes of A,B,C,D must fit")
# create a block matrix [[A,B], [C,D]] for visual representation
self.BlockRepresent = BlockMatrix(
[[self.represent[0], self.represent[1]],
[self.represent[2], self.represent[3]]])
return None
except TypeError:
raise TypeError("entries of 'representation' must be matrices")
except AttributeError:
raise TypeError("entries of 'representation' must be matrices")
except IndexError:
raise TypeError(
"'representation' must have at least 4 matrix-valued entries"
)
def test_block_index_symbolic_fail():
# To make this work, symbolic matrix dimensions would need to be somehow assumed nonnegative
# even if the symbols aren't specified as such. Then 2 * n < n would correctly evaluate to
# False in BlockMatrix._entry()
A1 = MatrixSymbol('A1', n, 1)
A2 = MatrixSymbol('A2', m, 1)
A = BlockMatrix([[A1], [A2]])
assert A[2 * n, 0] == A2[n, 0]
def test_ex93():
A = BlockMatrix([
[Identity(3)],
[-Identity(3)],
]).as_explicit()
b = Matrix(6*[1])
c = Matrix([0, 0, 1])
v0 = Matrix(3*[0])
v_start1 = determine_feasible_vertex_kernel(A, b, c, v0)
def is_feasible_eq(A: Matrix, b: Matrix):
"""
Checks feasibility of Ax = b by comparing ranks of A and (A,b)
"""
A_b: Matrix = BlockMatrix([A, b]).as_explicit()
A_r = A.rank()
A_b_r = A_b.rank()
if A_r == A_b_r:
return True
return False
def test_16857():
if not np:
skip("NumPy not installed")
a_1 = MatrixSymbol('a_1', 10, 3)
a_2 = MatrixSymbol('a_2', 10, 3)
a_3 = MatrixSymbol('a_3', 10, 3)
a_4 = MatrixSymbol('a_4', 10, 3)
A = BlockMatrix([[a_1, a_2], [a_3, a_4]])
assert A.shape == (20, 6)
printer = NumPyPrinter()
assert printer.doprint(A) == 'numpy.block([[a_1, a_2], [a_3, a_4]])'
def P_without(A: Matrix, b: Matrix, B: List[Matrix]):
""" Fix the space spanned by the vectors in B to 0 """
if len(B) == 0:
return A, b
B_rows = len(B)
B_mat = Matrix(B).transpose()
A_new = BlockMatrix([
[A],
[B_mat],
[-B_mat],
]).as_explicit()
b_new = Matrix(list(b) + 2*B_rows*[0])
return A_new, b_new
def test_block_index_large():
n, m, k = symbols('n m k', integer=True, positive=True)
i = symbols('i', integer=True, nonnegative=True)
A1 = MatrixSymbol('A1', n, n)
A2 = MatrixSymbol('A2', n, m)
A3 = MatrixSymbol('A3', n, k)
A4 = MatrixSymbol('A4', m, n)
A5 = MatrixSymbol('A5', m, m)
A6 = MatrixSymbol('A6', m, k)
A7 = MatrixSymbol('A7', k, n)
A8 = MatrixSymbol('A8', k, m)
A9 = MatrixSymbol('A9', k, k)
A = BlockMatrix([[A1, A2, A3], [A4, A5, A6], [A7, A8, A9]])
assert A[n + i, n + i] == MatrixElement(A, n + i, n + i)
def test_block_index():
I = Identity(3)
Z = ZeroMatrix(3, 3)
B = BlockMatrix([[I, I], [I, I]])
e3 = ImmutableMatrix(eye(3))
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B[0, 0] == B[3, 0] == B[0, 3] == B[3, 3] == 1
assert B[4, 3] == B[5, 1] == 0
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B.as_explicit() == BB.as_explicit()
BI = BlockMatrix([[I, Z], [Z, I]])
assert BI.as_explicit().equals(eye(6))
def test_block_index_symbolic_nonzero():
# All invalid simplifications from test_block_index_symbolic() that become valid if all
# matrices have nonzero size and all indices are nonnegative
k, l, m, n = symbols('k l m n', integer=True, positive=True)
i, j = symbols('i j', integer=True, nonnegative=True)
A1 = MatrixSymbol('A1', n, k)
A2 = MatrixSymbol('A2', n, l)
A3 = MatrixSymbol('A3', m, k)
A4 = MatrixSymbol('A4', m, l)
A = BlockMatrix([[A1, A2], [A3, A4]])
assert A[0, 0] == A1[0, 0]
assert A[n + m - 1, 0] == A3[m - 1, 0]
assert A[0, k + l - 1] == A2[0, l - 1]
assert A[n + m - 1, k + l - 1] == A4[m - 1, l - 1]
assert A[i, j] == MatrixElement(A, i, j)
assert A[n + i, k + j] == A4[i, j]
assert A[n - i - 1, k - j - 1] == A1[n - i - 1, k - j - 1]
assert A[2 * n, 2 * k] == A4[n, k]
def generate():
rotvec = MatrixSymbol("rotvec", 3, 1)
t = MatrixSymbol("t", 3, 1)
pose = BlockMatrix([[rotvec], [t]])
point = MatrixSymbol("point", 3, 1)
x_symbols = transform_project_symbols(rotvec, t, point)
generate_c_code("_transform_project", x_symbols,
prefix="tadataka/_transform_project/_transform_project")
generate_c_code("_pose_jacobian", x_symbols.jacobian(pose),
prefix="tadataka/_transform_project/_pose_jacobian")
generate_c_code("_point_jacobian", x_symbols.jacobian(point),
prefix="tadataka/_transform_project/_point_jacobian")
generate_c_code("_exp_so3", exp_so3_symbols(rotvec),
prefix="tadataka/_transform_project/_exp_so3")
def test_block_index_symbolic():
# Note that these matrices may be zero-sized and indices may be negative, which causes
# all naive simplifications given in the comments to be invalid
A1 = MatrixSymbol('A1', n, k)
A2 = MatrixSymbol('A2', n, l)
A3 = MatrixSymbol('A3', m, k)
A4 = MatrixSymbol('A4', m, l)
A = BlockMatrix([[A1, A2], [A3, A4]])
assert A[0, 0] == MatrixElement(A, 0, 0) # Cannot be A1[0, 0]
assert A[n - 1, k - 1] == A1[n - 1, k - 1]
assert A[n, k] == A4[0, 0]
assert A[n + m - 1, 0] == MatrixElement(A, n + m - 1,
0) # Cannot be A3[m - 1, 0]
assert A[0,
k + l - 1] == MatrixElement(A, 0,
k + l - 1) # Cannot be A2[0, l - 1]
assert A[n + m - 1, k + l - 1] == MatrixElement(
A, n + m - 1, k + l - 1) # Cannot be A4[m - 1, l - 1]
assert A[i, j] == MatrixElement(A, i, j)
assert A[n + i, k + j] == MatrixElement(A, n + i,
k + j) # Cannot be A4[i, j]
assert A[n - i - 1, k - j - 1] == MatrixElement(
A, n - i - 1, k - j - 1) # Cannot be A1[n - i - 1, k - j - 1]
def _inv_block(M, iszerofunc=_iszero):
"""Calculates the inverse using BLOCKWISE inversion.
See Also
========
inv
inverse_ADJ
inverse_GE
inverse_CH
inverse_LDL
"""
from sympy import BlockMatrix
i = M.shape[0]
if i <= 20:
return M.inv(method="LU", iszerofunc=_iszero)
A = M[:i // 2, :i // 2]
B = M[:i // 2, i // 2:]
C = M[i // 2:, :i // 2]
D = M[i // 2:, i // 2:]
try:
D_inv = _inv_block(D)
except NonInvertibleMatrixError:
return M.inv(method="LU", iszerofunc=_iszero)
B_D_i = B * D_inv
BDC = B_D_i * C
A_n = A - BDC
try:
A_n = _inv_block(A_n)
except NonInvertibleMatrixError:
return M.inv(method="LU", iszerofunc=_iszero)
B_n = -A_n * B_D_i
dc = D_inv * C
C_n = -dc * A_n
D_n = D_inv + dc * -B_n
nn = BlockMatrix([[A_n, B_n], [C_n, D_n]]).as_explicit()
return nn
def identifyFeasibleBaseParameters(self, idf):
# type: (Identification) -> None
''' use SDP optimization to solve OLS to find physically feasible base parameters (i.e. for
which a consistent std solution exists), based on code from github.com/cdsousa/wam7_dyn_ident
'''
with helpers.Timer() as t:
if idf.opt['verbose']:
print("Preparing SDP...")
# build OLS matrix
I = Identity
# base and standard parameter symbols
delta = Matrix(idf.model.param_syms)
beta_symbs = idf.model.base_syms
# permutation of std to base columns projection
# (simplify to reduce 1.0 to 1 etc., important for replacement)
Pb = Matrix(idf.model.Pb).applyfunc(lambda x: x.nsimplify())
# permutation of std to non-identifiable columns (dependents)
Pd = Matrix(idf.model.Pd).applyfunc(lambda x: x.nsimplify())
# projection matrix from independents to dependents
#Kd = Matrix(idf.model.linear_deps)
#K = Matrix(idf.model.K).applyfunc(lambda x: x.nsimplify()) #(Pb.T + Kd * Pd.T)
# equations for base parameters expressed in independent std param symbols
#beta = K * delta
beta = Matrix(idf.model.base_deps).applyfunc(lambda x: x.nsimplify())
# std vars that occur in base params (as many as base params, so only the single ones or
# chosen as independent ones)
if idf.opt['useBasisProjection']:
# determined through base matrix, which included other variables too
# (find first variable in eq, chosen as independent here)
delta_b_syms = [] # type: List[sympy.Symbol]
for i in range(idf.model.base_deps.shape[0]):
for s in idf.model.base_deps[i].free_symbols:
if s not in delta_b_syms:
delta_b_syms.append(s)
break
delta_b = Matrix(delta_b_syms)
else:
# determined through permutation matrix from QR (not correct if base matrix is orthogonalized afterwards)
delta_b = Pb.T*delta
# std variables that are dependent, i.e. their value is a combination of independent columns
# (they don't appear in base params but in feasibility constraints)
if idf.opt['useBasisProjection']:
#determined from base eqns
delta_not_d = idf.model.base_deps[0].free_symbols
for e in idf.model.base_deps:
delta_not_d = delta_not_d.union(e.free_symbols)
delta_d_syms = []
for s in delta:
if s not in delta_not_d:
delta_d_syms.append(s)
delta_d = Matrix(delta_d_syms)
else:
# determined through permutation matrix from QR (not correct if base matrix is orthogonalized afterwards)
delta_d = Pd.T*delta
# rewrite LMIs for base params
if idf.opt['useBasisProjection']:
# (Sousa code is assuming that delta_b for each eq has factor 1.0 in equations beta.
# this is true if using Gautier dependency matrix, otherwise
# correct is to properly transpose eqn base_n = a1*x1 + a2*x2 + ... +an*xn to
# 1*xi = a1*x1/ai + a2*x2/ai + ... + an*xn/ai - base_n/ai )
transposed_beta = Matrix([solve(beta[i], delta_b[i])[0] for i in range(len(beta))])
self.varchange_dict = dict(zip(delta_b, beta_symbs + transposed_beta))
#add free vars to variables for optimization
for eq in transposed_beta:
for s in eq.free_symbols:
if s not in delta_d:
delta_d = delta_d.col_join(Matrix([s]))
else:
self.varchange_dict = dict(zip(delta_b, beta_symbs - (beta - delta_b)))
DB_blocks = [self.mrepl(Di, self.varchange_dict) for Di in self.D_blocks]
self.DB_LMIs_marg = list([LMI_PSD(lm - self.epsilon_safemargin*eye(lm.shape[0])) for lm in DB_blocks])
Q, R = la.qr(idf.model.YBase)
#Q1 = Q[:, 0:idf.model.num_base_params]
#Q2 = Q[:, idf.model.num_base_params:]
R1 = np.matrix(R[:idf.model.num_base_params, :idf.model.num_base_params]) # type: np.matrix[float]
# OLS: minimize ||tau - Y*x_base||^2 (simplify)=> minimize ||rho1.T - R1*K*delta||^2
rho1 = Q.T.dot(idf.model.torques_stack - idf.model.contactForcesSum)
e_rho1 = Matrix(rho1) - (R1*beta_symbs)
rho2_norm_sqr = la.norm(idf.model.torques_stack - idf.model.YBase.dot(idf.model.xBase))**2
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
[e_rho1, I(idf.model.num_base_params)]])
U_rho = U_rho.as_explicit()
if idf.opt['verbose']:
print("Add constraint LMIs")
lmis = [LMI_PSD(U_rho)] + self.DB_LMIs_marg
variables = [u] + list(beta_symbs) + list(delta_d)
# solve SDP
objective_func = u
if idf.opt['verbose']:
print("Solving constrained OLS as SDP")
# start at CAD data, might increase convergence speed (atm only works with dsdp5.
# with cvxopt, only returns primal as solution when failing)
prime = np.concatenate((idf.model.xBaseModel, np.array(Pd.T*idf.model.xStdModel)[:,0]))
onlyUseDSDP = 0
if not onlyUseDSDP:
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime)
#try again with wider bounds and dsdp5 cmd line
if onlyUseDSDP or state is not 'optimal':
print("Trying again with dsdp5 solver")
sdp_helpers.solve_sdp = sdp_helpers.dsdp5
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime, wide_bounds=True)
sdp_helpers.solve_sdp = sdp_helpers.cvxopt_conelp
u = solution[0,0]
if u:
print("SDP found base solution with {} error increase from OLS solution".format(u))
beta_star = np.matrix(solution[1:1+idf.model.num_base_params]) # type: np.matrix[float]
idf.model.xBase = np.squeeze(np.asarray(beta_star))
if idf.opt['showTiming']:
print("Constrained SDP optimization took %.03f sec." % (t.interval))
def identifyFeasibleStandardParametersDirect(self, idf):
# type: (Identification) -> None
''' use SDP optimzation to solve constrained OLS to find globally optimal physically
feasible std parameters. Based on code from Sousa, 2014
'''
with helpers.Timer() as t:
#if idf.opt['useAPriori']:
# print("Please disable using a priori parameters when using constrained optimization.")
# sys.exit(1)
if idf.opt['verbose']:
print("Preparing SDP...")
# build OLS matrix
I = Identity
delta = Matrix(idf.model.param_syms)
YStd = idf.model.YStd #idf.YStd_nonsing
tau = idf.model.torques_stack
p_nid = idf.model.non_id
if idf.opt['useRegressorRegularization'] and len(p_nid):
#p_nid = list(set(p_nid).difference(set(self.delete_cols)))
#l = [0.001]*len(p_nid)
l = [(float(idf.base_error) / len(p_nid)) * 1.5]*len(p_nid) #proportion of distance term
YStd = np.vstack((YStd, (l*np.identity(idf.model.num_identified_params)[p_nid].T).T))
tau = np.concatenate((tau, l*idf.model.xStdModel[p_nid]))
for c in reversed(self.delete_cols):
delta.row_del(c)
YStd = np.delete(YStd, self.delete_cols, axis=1)
Q, R = la.qr(YStd)
Q1 = Q[:, 0:idf.model.num_identified_params]
#Q2 = Q[:, idf.model.num_base_params:]
rho1 = Q1.T.dot(tau)
R1 = np.matrix(R[:idf.model.num_identified_params, :idf.model.num_identified_params])
# OLS: minimize ||tau - Y*x_base||^2 (simplify)=> minimize ||rho1.T - R1*K*delta||^2
# add contact forces
if idf.opt['useRegressorRegularization']:
contactForcesSum = np.concatenate((idf.model.contactForcesSum, np.zeros(len(p_nid))))
else:
contactForcesSum = idf.model.contactForcesSum
contactForces = Matrix(Q.T.dot(contactForcesSum))
if idf.opt['verbose'] > 1:
print("Step 1...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
e_rho1 = Matrix(rho1 - contactForces) - (R1*delta)
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# calc estimation error of previous OLS parameter solution
rho2_norm_sqr = la.norm(idf.model.torques_stack - idf.model.YBase.dot(idf.model.xBase))**2
# (this is the slow part when matrices get bigger, BlockMatrix or as_explicit?)
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
[e_rho1, I(idf.model.num_identified_params)]])
if idf.opt['verbose'] > 1:
print("Step 3...", time.ctime())
U_rho = U_rho.as_explicit()
if idf.opt['verbose'] > 1:
print("Step 4...", time.ctime())
if idf.opt['verbose']:
print("Add constraint LMIs")
lmis = [LMI_PSD(U_rho)] + self.LMIs_marg
variables = [u] + list(delta)
#solve SDP
objective_func = u
if idf.opt['verbose']:
print("Solving constrained OLS as SDP")
# start at CAD data, might increase convergence speed (atm only works with dsdp5,
# otherwise returns primal as solution when failing)
prime = idf.model.xStdModel
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime)
#try again with wider bounds and dsdp5 cmd line
if state is not 'optimal':
print("Trying again with dsdp5 solver")
sdp_helpers.solve_sdp = sdp_helpers.dsdp5
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime, wide_bounds=True)
sdp_helpers.solve_sdp = sdp_helpers.cvxopt_conelp
u = solution[0,0]
if u:
print("SDP found std solution with {} squared residual error".format(u))
delta_star = np.matrix(solution[1:])
idf.model.xStd = np.squeeze(np.asarray(delta_star))
#prepend apriori values for 0'th link non-identifiable variables
for c in self.delete_cols:
idf.model.xStd = np.insert(idf.model.xStd, c, 0)
idf.model.xStd[self.delete_cols] = idf.model.xStdModel[self.delete_cols]
if idf.opt['showTiming']:
print("Constrained SDP optimization took %.03f sec." % (t.interval))
def identifyFeasibleStandardParameters(self, idf):
# type: (Identification) -> None
''' use SDP optimization to solve constrained OLS to find globally optimal physically
feasible std parameters (not necessarily unique). Based on code from Sousa, 2014
'''
with helpers.Timer() as t:
if idf.opt['verbose']:
print("Preparing SDP...")
I = Identity
delta = Matrix(idf.model.param_syms[idf.model.identified_params])
# ignore some params that are non-identifiable
for c in reversed(self.delete_cols):
delta.row_del(c)
YBase = idf.model.YBase
tau = idf.model.torques_stack # always absolute torque values
# get projection matrix so that xBase = K*xStd
if idf.opt['useBasisProjection']:
K = Matrix(idf.model.Binv)
else:
# Sousa: K = Pb.T + Kd * Pd.T (Kd==idf.model.linear_deps, [Pb Pd] == idf.model.Pp)
# Pb = Matrix(idf.model.Pb) #.applyfunc(lambda x: x.nsimplify())
# Pd = Matrix(idf.model.Pd) #.applyfunc(lambda x: x.nsimplify())
K = Matrix(idf.model.K) #(Pb.T + Kd * Pd.T)
for c in reversed(self.delete_cols):
K.col_del(c)
Q, R = la.qr(YBase)
Q1 = Q[:, 0:idf.model.num_base_params]
R1 = np.matrix(R[:idf.model.num_base_params, :idf.model.num_base_params])
rho1 = Q1.T.dot(tau)
contactForces = Q.T.dot(idf.model.contactForcesSum)
if idf.opt['useRegressorRegularization']:
p_nid = idf.model.non_id
p_nid = list(set(p_nid).difference(set(self.delete_cols)).intersection(set(idf.model.identified_params)))
contactForces = np.concatenate((contactForces, np.zeros(len(p_nid))))
if idf.opt['verbose'] > 1:
print("Step 1...", time.ctime())
# solving OLS: minimize ||tau - Y*x_base||^2 (simplify)=> minimize ||rho1.T - R1*K*delta||^2
# get upper bound for regression error
# rho2_norm_sqr = la.norm(Q2.T.dot(tau))**2 = 0
# since we use QR if YBase, Q2 is empty anyway, so rho2 = Q2*tau following the paper is zero
# the code from sousa's notebook includes a different calculation for the upper bound:
rho2_norm_sqr = la.norm(idf.model.torques_stack - idf.model.contactForcesSum - idf.model.YBase.dot(idf.model.xBase))**2
# get additional regression error
if idf.opt['useRegressorRegularization'] and len(p_nid):
# add regularization term to cost function to include torque estimation error and CAD distance
# get symbols that are non-id but are not in delete_cols already
delta_nonid = Matrix(idf.model.param_syms[p_nid])
#num_samples = YBase.shape[0]/idf.model.num_dofs
l = (float(idf.base_error) / len(p_nid)) * idf.opt['regularizationFactor']
#TODO: also use symengine to gain speedup?
#p = BlockMatrix([[(K*delta)], [delta_nonid]])
#Y = BlockMatrix([[Matrix(R1), ZeroMatrix(R1.shape[0], len(p_nid))],
# [ZeroMatrix(len(p_nid), R1.shape[1]), l*Identity(len(p_nid))]])
Y = BlockMatrix([[R1*(K*delta)],[l*Identity(len(p_nid))*delta_nonid]]).as_explicit()
rho1_hat = np.concatenate((rho1, l*idf.model.xStdModel[p_nid]))
e_rho1 = (Matrix(rho1_hat - contactForces) - Y)
else:
try:
from symengine import DenseMatrix as eMatrix
if idf.opt['verbose']:
print('using symengine')
edelta = eMatrix(delta.shape[0], delta.shape[1], delta)
eK = eMatrix(K.shape[0], K.shape[1], K)
eR1 = eMatrix(R1.shape[0], R1.shape[1], Matrix(R1))
Y = eR1*eK*edelta
e_rho1 = Matrix(eMatrix(rho1) - contactForces - Y)
except ImportError:
if idf.opt['verbose']:
print('not using symengine')
Y = R1*(K*delta)
e_rho1 = Matrix(rho1 - contactForces) - Y
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
[e_rho1, I(e_rho1.shape[0])]])
if idf.opt['verbose'] > 1:
print("Step 3...", time.ctime())
U_rho = U_rho.as_explicit()
if idf.opt['verbose'] > 1:
print("Step 4...", time.ctime())
if idf.opt['verbose']:
print("Add constraint LMIs")
lmis = [LMI_PSD(U_rho)] + self.LMIs_marg
variables = [u] + list(delta)
objective_func = u
# solve SDP
# start at CAD data, might increase convergence speed (atm only works with dsdp5,
# but is used to return primal as solution when failing cvxopt)
if idf.opt['verbose']:
print("Solving constrained OLS as SDP")
idable_params = sorted(list(set(idf.model.identified_params).difference(self.delete_cols)))
prime = idf.model.xStdModel[idable_params]
if idf.opt['checkAPrioriFeasibility']:
self.checkFeasibility(idf.model.xStdModel)
idf.opt['onlyUseDSDP'] = 0
if not idf.opt['onlyUseDSDP']:
if idf.opt['verbose']:
print("Solving with cvxopt...", end=' ')
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime)
# try again with wider bounds and dsdp5 cmd line
if idf.opt['onlyUseDSDP'] or state is not 'optimal':
if idf.opt['verbose']:
print("Solving with dsdp5...", end=' ')
sdp_helpers.solve_sdp = sdp_helpers.dsdp5
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=prime, wide_bounds=True)
sdp_helpers.solve_sdp = sdp_helpers.cvxopt_conelp
u = solution[0, 0]
if u:
print("SDP found std solution with {} squared residual error".format(u))
delta_star = np.matrix(solution[1:]) # type: np.matrix
idf.model.xStd = np.squeeze(np.asarray(delta_star))
# prepend apriori values for 0'th link non-identifiable variables
for c in self.delete_cols:
idf.model.xStd = np.insert(idf.model.xStd, c, 0)
idf.model.xStd[self.delete_cols] = idf.model.xStdModel[self.delete_cols]
if idf.opt['showTiming']:
print("Constrained SDP optimization took %.03f sec." % (t.interval))
def test_block_index():
I = Identity(3)
Z = ZeroMatrix(3, 3)
B = BlockMatrix([[I, I], [I, I]])
e3 = ImmutableMatrix(eye(3))
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B[0, 0] == B[3, 0] == B[0, 3] == B[3, 3] == 1
assert B[4, 3] == B[5, 1] == 0
BB = BlockMatrix([[e3, e3], [e3, e3]])
assert B.as_explicit() == BB.as_explicit()
BI = BlockMatrix([[I, Z], [Z, I]])
assert BI.as_explicit().equals(eye(6))
def _describe(Op):
"""Core steps to describe a composite operator. This is done recursively.
Parameters
----------
Op : :obj:`pylops.LinearOperator`
Linear Operator to assign name to
Ops : :obj:`dict`
Dictionary of Operators found by the _describe method whilst crawling
through the composite operator to describe
names : :obj:`list`
List of currently assigned names
"""
Ops = {}
if type(Op) not in compositeops:
# A native PyLops operator has been found, assign a name and store
# it as MatrixSymbol
name = _assign_name(Op, Ops, list(Ops.keys()))
Ops[name] = (type(Op).__name__, id(Op))
Opsym = MatrixSymbol(Op.name, 1, 1)
else:
if type(Op) == LinearOperator:
# A LinearOperator has been found, either extract Op and start to
# describe it or if a name has been given to the operator treat as
# it is (this is useful when users do not want an operator to be
# further disected into its components
name = getattr(Op, "name", None)
if name is None:
Opsym, Ops_ = _describe(Op.Op)
Ops.update(Ops_)
else:
Ops[name] = (type(Op).__name__, id(Op))
Opsym = MatrixSymbol(Op.name, 1, 1)
elif type(Op) == _AdjointLinearOperator:
# An adjoint LinearOperator has been found, describe it and attach
# the adjoint symbol to its sympy representation
Opsym, Ops_ = _describe(Op.args[0])
Opsym = Opsym.adjoint()
Ops.update(Ops_)
elif type(Op) == _TransposedLinearOperator:
# A transposed LinearOperator has been found, describe it and
# attach the transposed symbol to its sympy representation
Opsym, Ops_ = _describe(Op.args[0])
Opsym = Opsym.T
Ops.update(Ops_)
elif type(Op) == _ScaledLinearOperator:
# A scaled LinearOperator has been found, describe it and
# attach the scaling to its sympy representation. Note that the
# scaling could either on the left or right side of the operator,
# so we need to try both
if isinstance(Op.args[0], LinearOperator):
Opsym, Ops_ = _describeop(Op.args[0], Ops, list(Ops.keys()))
Opsym = Op.args[1] * Opsym
Ops.update(Ops_)
else:
Opsym, Ops_ = _describeop(Op.args[1], Ops, list(Ops.keys()))
Opsym = Op.args[1] * Opsym
Ops.update(Ops_)
elif type(Op) == _SumLinearOperator:
# A sum LinearOperator has been found, describe both operators
# either side of the plus sign and sum their sympy representations
Opsym0, Ops_ = _describeop(Op.args[0], Ops, list(Ops.keys()))
Ops.update(Ops_)
Opsym1, Ops_ = _describeop(Op.args[1], Ops, list(Ops.keys()))
Ops.update(Ops_)
Opsym = Opsym0 + Opsym1
elif type(Op) == _ProductLinearOperator:
# Same as sum LinearOperator but for product
Opsym0, Ops_ = _describeop(Op.args[0], Ops, list(Ops.keys()))
Ops.update(Ops_)
Opsym1, Ops_ = _describeop(Op.args[1], Ops, list(Ops.keys()))
Ops.update(Ops_)
Opsym = Opsym0 * Opsym1
elif type(Op) in (VStack, HStack, BlockDiag):
# A special composite operator has been found, stack its components
# horizontally, vertically, or along a diagonal
Opsyms = []
for op in Op.ops:
Opsym, Ops_ = _describeop(op, Ops, list(Ops.keys()))
Opsyms.append(Opsym)
Ops.update(Ops_)
Ops.update(Ops_)
if type(Op) == VStack:
Opsym = BlockMatrix([[Opsym] for Opsym in Opsyms])
elif type(Op) == HStack:
Opsym = BlockMatrix(Opsyms)
elif type(Op) == BlockDiag:
Opsym = BlockDiagMatrix(*Opsyms)
return Opsym, Ops
def getModel():
# ---------- STATE (S) -----------
#variables
x, y, p = symbols('x y p')
S_k = [x, y, p]
# ---------- INPUTS (U) -----------
#variables
v, d = symbols('v d')
U_k = Matrix([v, d])
# ---------- STATE LINEARIZATION (Sbar) -----------
xb, yb, pb = symbols('xb yb pb')
Sbar = Matrix([xb, yb, pb])
# ---------- INPUT LINEARIZATION (Ubar) -----------
vb, db = symbols('vb db')
Ubar = Matrix([vb, db])
# ---------- BOUNDS -----------
vs, ds = symbols('vs ds')
boundVars = Matrix([vs, ds])
bounds_rhs = Matrix([vb + vs, -(vb - vs), db + ds, -(db - ds)])
# ---------- CONSTANTS -----------
#variables
L, dt = symbols('L dt')
L0 = .32
constantVals = [L0]
constantSym = Matrix([L, dt])
# F(Sbar, UBar)
F = Matrix([vb * cos(pb + db), vb * sin(pb + db), (vb / L) * sin(db)])
#find partials
dFdS = F.jacobian(Sbar)
dFdU = F.jacobian(Ubar)
#construct constant matrix
C = F - (dFdS * Sbar) - (dFdU * Ubar)
#define final state/inputs
S = S_k + [1] * C.shape[0]
U = U_k
#make pieces of A-matrix and combine (state dynamics)
M1 = dFdS #+ eye(Sbar.shape[0])
M2 = diag(*C)
M3 = zeros(Sbar.shape[0])
M4 = zeros(C.shape[0])
A_mat = Matrix(BlockMatrix([[M1, M2], [M3, M4]]))
#Assemble B-matrix (input dynamics)
B_mat = Matrix([dFdU, zeros(C.shape[0], U.shape[0])])
init_printing()
#pprint(F)
#pprint(dFdU * Ubar)
#pprint(F - dFdU * Ubar)
#pprint(dFdS * Sbar)
#pprint(C)
#pprint(A_mat)
#pprint(B_mat)
all_vars = list(Sbar) + list(Ubar) + list(constantSym) + list(boundVars)
return Model(all_vars, constantVals, A_mat, B_mat, bounds_rhs)
def findFeasibleStdFromFeasibleBase(self, idf, xBase):
# type: (Identification, np._ArrayLike) -> None
''' find a std feasible solution for feasible base solution (exists by definition) while
minimizing param distance to a-priori parameters
'''
with helpers.Timer() as t:
I = Identity
# symbols for std params
idable_params = sorted(list(set(idf.model.identified_params).difference(self.delete_cols)))
delta = Matrix(idf.model.param_syms[idable_params])
# equations for base parameters expressed in independent std param symbols
beta = idf.model.base_deps #.applyfunc(lambda x: x.nsimplify())
# add explicit constraints for each base param equation and estimated value
D_base_val_blocks = []
for i in range(idf.model.num_base_params):
D_base_val_blocks.append(Matrix([beta[i] - (xBase[i] - self.epsilon_safemargin)]))
D_base_val_blocks.append(Matrix([xBase[i] + (self.epsilon_safemargin - beta[i])]))
self.D_blocks += D_base_val_blocks
self.LMIs_marg = list([LMI_PSD(lm - self.epsilon_safemargin*eye(lm.shape[0])) for lm in self.D_blocks])
# closest to CAD but ignore non_identifiable params
sol_cad_dist = Matrix(idf.model.xStdModel[idable_params]) - delta
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u]), sol_cad_dist.T],
[sol_cad_dist, I(len(idable_params))]])
U_rho = U_rho.as_explicit()
lmis = [LMI_PSD(U_rho)] + self.LMIs_marg
variables = [u] + list(delta)
objective_func = u # 'find' problem
xStd = np.delete(idf.model.xStd, self.delete_cols)
old_dist = la.norm(idf.model.xStdModel[idable_params] - xStd)**2
if idf.opt['checkAPrioriFeasibility']:
self.checkFeasibility(idf.model.xStd)
# don't use cvxopt atm because it won't use primal and fail anyway
onlyUseDSDP = 1
if not onlyUseDSDP:
if idf.opt['verbose']:
print("Solving with cvxopt...", end=' ')
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=xStd)
# try again with wider bounds and dsdp5 cmd line
if onlyUseDSDP or state is not 'optimal':
if idf.opt['verbose']:
print("Solving with dsdp5...", end=' ')
sdp_helpers.solve_sdp = sdp_helpers.dsdp5
# start at CAD data to find solution faster
solution, state = sdp_helpers.solve_sdp(objective_func, lmis, variables, primalstart=xStd, wide_bounds=True)
sdp_helpers.solve_sdp = sdp_helpers.cvxopt_conelp
u = solution[0, 0]
print("SDP found std solution with distance {} from CAD solution (compared to {})".format(u, old_dist))
idf.model.xStd = np.squeeze(np.asarray(solution[1:]))
# prepend apriori values for 0'th link non-identifiable variables
for c in self.delete_cols:
idf.model.xStd = np.insert(idf.model.xStd, c, 0)
idf.model.xStd[self.delete_cols] = idf.model.xStdModel[self.delete_cols]
if idf.opt['showTiming']:
print("Constrained SDP optimization took %.03f sec." % (t.interval))
def initSDP_LMIs(self, idf, remove_nonid=True):
# type: (Identification, bool) -> None
''' initialize LMI matrices to set physical consistency constraints for SDP solver
based on Sousa, 2014 and corresponding code (https://github.com/cdsousa/IROS2013-Feas-Ident-WAM7)
'''
with helpers.Timer() as t:
if idf.opt['verbose']:
print("Initializing LMIs...")
def skew(v):
return Matrix([[ 0,-v[2], v[1]],
[ v[2], 0,-v[0]],
[-v[1], v[0], 0 ]])
I = Identity
S = skew
# don't include equations for 0'th link (in case it's fixed)
if idf.opt['floatingBase'] == 0 and idf.opt['deleteFixedBase']:
if idf.opt['identifyGravityParamsOnly']:
self.delete_cols = [0,1,2,3]
else:
self.delete_cols = [0,1,2,3,4,5,6,7,8,9]
if set(self.delete_cols).issubset(idf.model.non_id):
start_link = 1
else:
# if 0'th link is identifiable, just include as usual
start_link = 0
self.delete_cols = []
else:
start_link = 0
self.delete_cols = []
D_inertia_blocks = []
D_other_blocks = []
if idf.opt['identifyGravityParamsOnly']:
# only constrain gravity params (i.e. mass as COM is not part of physical
# consistency)
for i in range(start_link, idf.model.num_links):
if i*10 not in self.delete_cols:
p = idf.model.mass_params[i]
D_other_blocks.append(Matrix([idf.model.param_syms[p]]))
else:
# create LMI matrices (symbols) for each link
# so that mass is positive, inertia matrix is positive definite
# (each matrix block is constrained to be >0 or >=0)
for i in range(start_link, idf.model.num_links):
m = idf.model.mass_syms[i]
l = Matrix([idf.model.param_syms[i*10+1],
idf.model.param_syms[i*10+2],
idf.model.param_syms[i*10+3]])
L = Matrix([[idf.model.param_syms[i*10+4+0],
idf.model.param_syms[i*10+4+1],
idf.model.param_syms[i*10+4+2]],
[idf.model.param_syms[i*10+4+1],
idf.model.param_syms[i*10+4+3],
idf.model.param_syms[i*10+4+4]],
[idf.model.param_syms[i*10+4+2],
idf.model.param_syms[i*10+4+4],
idf.model.param_syms[i*10+4+5]]
])
Di = BlockMatrix([[L, S(l).T],
[S(l), I(3)*m]])
D_inertia_blocks.append(Di.as_mutable())
params_to_skip = []
# ignore depending on sub-regressor condition numbers per link
if idf.opt['noChange']:
linkConds = idf.model.getSubregressorsConditionNumbers()
robotmass_apriori = 0
for i in range(0, idf.model.num_links):
robotmass_apriori += idf.model.xStdModel[i*10] #count a priori link masses
# for links that have too high condition number, don't change params
if idf.opt['noChange'] and linkConds[i] > idf.opt['noChangeThresh']:
print(Fore.YELLOW + 'not changing parameters of link {} ({})!'.format(i, idf.model.linkNames[i]) + Fore.RESET)
# don't change mass
params_to_skip.append(i*10)
# don't change COM
params_to_skip.append(i*10+1)
params_to_skip.append(i*10+2)
params_to_skip.append(i*10+3)
# don't change inertia
params_to_skip.append(i*10+4)
params_to_skip.append(i*10+5)
params_to_skip.append(i*10+6)
params_to_skip.append(i*10+7)
params_to_skip.append(i*10+8)
params_to_skip.append(i*10+9)
# constrain manually fixed params
for p in idf.opt['dontChangeParams']:
params_to_skip.append(p)
for p in set(params_to_skip):
if (idf.opt['identifyGravityParamsOnly'] and p not in idf.model.inertia_params) \
or not idf.opt['identifyGravityParamsOnly']:
if p not in idf.opt['dontConstrain']:
D_other_blocks.append(Matrix([idf.model.xStdModel[p] - idf.model.param_syms[p]]))
D_other_blocks.append(Matrix([idf.model.param_syms[p] - idf.model.xStdModel[p]]))
self.constr_per_param[p].append('cad')
# constrain overall mass within bounds
if idf.opt['limitOverallMass']:
# use given overall mass else use overall mass from CAD
if idf.opt['limitMassVal']:
robotmaxmass = idf.opt['limitMassVal'] - sum(idf.model.xStdModel[0:start_link*10:10])
robotmaxmass_ub = robotmaxmass * 1.0 + idf.opt['limitMassRange']
robotmaxmass_lb = robotmaxmass * 1.0 - idf.opt['limitMassRange']
else:
robotmaxmass = robotmass_apriori
# constrain within apriori range
robotmaxmass_ub = robotmaxmass * 1.0 + idf.opt['limitMassRange']
robotmaxmass_lb = robotmaxmass * 1.0 - idf.opt['limitMassRange']
D_other_blocks.append(Matrix([robotmaxmass_ub - sum(idf.model.mass_syms[start_link:])])) # maximum mass
D_other_blocks.append(Matrix([sum(idf.model.mass_syms[start_link:]) - robotmaxmass_lb])) # minimum mass
# start with pudb
#import pudb; pu.db
# constrain masses for each link separately
if idf.opt['limitMassToApriori']:
# constrain each mass to env of a priori value
for i in range(start_link, idf.model.num_links):
if not (idf.opt['noChange'] and linkConds[i] > idf.opt['noChangeThresh']):
p = i*10
if p not in idf.opt['dontConstrain']:
bound = np.abs(idf.model.xStdModel[p]) * idf.opt['limitMassAprioriBoundary']
#if np.abs(idf.model.xStdModel[p]) < 0.001:
# bound += 0.001
lb = Matrix([idf.model.mass_syms[i] -
(idf.model.xStdModel[p] - bound)])
ub = Matrix([-idf.model.mass_syms[i] +
(idf.model.xStdModel[p] + bound)])
D_other_blocks.append(lb)
D_other_blocks.append(ub)
self.constr_per_param[p].append('mA')
# constrain masses for each link separately
if idf.opt['limitCOMToApriori']:
# constrain each mass to env of a priori value
for i in range(start_link, idf.model.num_links):
if not (idf.opt['noChange'] and linkConds[i] > idf.opt['noChangeThresh']):
for p in range(i*10+1, i*10+4):
if p not in idf.opt['dontConstrain']:
bound = np.abs(idf.model.xStdModel[p]) * idf.opt['limitCOMAprioriBoundary']
if np.abs(idf.model.xStdModel[p]) < 0.01:
bound += 0.01
lb = Matrix([idf.model.param_syms[p] -
(idf.model.xStdModel[p] - bound)])
ub = Matrix([-idf.model.param_syms[p] +
(idf.model.xStdModel[p] + bound)])
D_other_blocks.append(lb)
D_other_blocks.append(ub)
self.constr_per_param[p].append('cA')
if idf.opt['restrictCOMtoHull']:
link_cuboid_hulls = {} # type: Dict[str, Tuple[List, List, List]]
for i in range(start_link, idf.model.num_links):
if not (idf.opt['noChange'] and linkConds[i] > idf.opt['noChangeThresh']):
link_name = idf.model.linkNames[i]
box, pos, rot = idf.urdfHelpers.getBoundingBox(
input_urdf = idf.model.urdf_file,
old_com = idf.model.xStdModel[i*10+1:i*10+4] / idf.model.xStdModel[i*10],
link_name = link_name
)
link_cuboid_hulls[link_name] = (box, pos, rot)
l = Matrix(idf.model.param_syms[i*10+1:i*10+4])
m = idf.model.mass_syms[i]
link_cuboid_hull = link_cuboid_hulls[link_name][0]
for j in range(3):
p = i*10+1+j
if p not in self.delete_cols and p not in idf.opt['dontConstrain']:
lb = Matrix([[ l[j] - m*link_cuboid_hull[0][j]]])
ub = Matrix([[-l[j] + m*link_cuboid_hull[1][j]]])
D_other_blocks.append(lb)
D_other_blocks.append(ub)
self.constr_per_param[p].append('hull')
elif not idf.opt['limitCOMToApriori'] and idf.opt['identifyGravityParamsOnly']:
print(Fore.RED+"COM parameters are not constrained,", end=' ')
print("might result in rank deficiency when solving SDP problem!"+Fore.RESET)
# symmetry constraints
if idf.opt['useSymmetryConstraints'] and idf.opt['symmetryConstraints']:
for (a, b, sign) in idf.opt['symmetryConstraints']:
if (idf.opt['identifyGravityParamsOnly'] and a not in idf.model.inertia_params and
b not in idf.model.inertia_params) \
or not idf.opt['identifyGravityParamsOnly']:
eps = idf.opt['symmetryTolerance']
p_a = idf.model.param_syms[a]
p_b = idf.model.param_syms[b]
# schur matrix of (a-b)^2 == (a-b)(a-b)^T < eps
sym = Matrix([[eps, p_a - sign*p_b],
[p_a - sign*p_b, 1]])
D_other_blocks.append(sym.as_mutable())
self.constr_per_param[a].append('sym')
self.constr_per_param[b].append('sym')
if idf.opt['identifyFriction']:
# friction constraints, need to be positive
# (only makes sense when no offsets on torque measurements and if there is
# movements, otherwise constant friction includes offsets and can be negative)
if not idf.opt['identifyGravityParamsOnly']:
for i in range(idf.model.num_dofs):
# Fc > 0
p = i #idf.model.num_model_params+i
#D_other_blocks.append( Matrix([idf.model.friction_syms[p]]) )
#self.constr_per_param[idf.model.num_model_params + p].append('>0')
# Fv > 0
D_other_blocks.append(Matrix([idf.model.friction_syms[p+idf.model.num_dofs]]))
self.constr_per_param[idf.model.num_model_params + p + idf.model.num_dofs].append('>0')
if not idf.opt['identifySymmetricVelFriction']:
D_other_blocks.append(Matrix([idf.model.friction_syms[p+idf.model.num_dofs*2]]))
self.constr_per_param[idf.model.num_model_params + p + idf.model.num_dofs * 2].append('>0')
self.D_blocks = D_inertia_blocks + D_other_blocks
self.LMIs = list(map(LMI_PD, self.D_blocks))
self.epsilon_safemargin = 1e-6
self.LMIs_marg = list([LMI_PSD(lm - self.epsilon_safemargin*eye(lm.shape[0])) for lm in self.D_blocks])
if idf.opt['showTiming']:
print("Initializing LMIs took %.03f sec." % (t.interval))
def bm(m):
"""Simple block formatted matrix constructor, returns a Matrix"""
return Matrix(BlockMatrix(m))
from sympy import MatrixSymbol, BlockMatrix, block_collapse, latex from sympy.abc import n A, B, C, D, E, F, G, H = [MatrixSymbol(a, n, n) for a in 'ABCDEFGH'] X = BlockMatrix([[A, B], [C, D]]) Y = BlockMatrix([[E, F], [G, H]]) print latex(X * Y) print latex(block_collapse(X * Y)) print latex(block_collapse(X.I))
def discretizeF(self):
Z33 = ZeroMatrix(3, 3)
Z31 = ZeroMatrix(3, 1)
Z13 = ZeroMatrix(1, 3)
Z11 = ZeroMatrix(1, 1)
I33 = Identity(3)
I11 = Identity(1)
F11 = Z33
F12 = MatrixSymbol('F12', 3, 3)
F13 = MatrixSymbol('F13', 3, 3)
F14 = Z33
F15 = Z31
F16 = Z31
F17 = Z33
F21 = Z33
F22 = MatrixSymbol('F22', 3, 3)
F23 = MatrixSymbol('F23', 3, 3)
F24 = MatrixSymbol('F24', 3, 3)
F25 = MatrixSymbol('F25', 3, 1)
F26 = Z31
F27 = Z33
F31 = Z33
F32 = Z33
F33 = MatrixSymbol('F33', 3, 3)
F34 = I33
F35 = Z31
F36 = Z31
F37 = Z33
F41 = Z33
F42 = Z33
F43 = Z33
F44 = MatrixSymbol('F44', 3, 3)
F45 = MatrixSymbol('F45', 3, 1)
F46 = MatrixSymbol('F46', 3, 1)
F47 = MatrixSymbol('F47', 3, 3)
F51 = Z13
F52 = Z13
F53 = Z13
F54 = Z13
F55 = Z11
F56 = Z11
F57 = Z13
F61 = Z13
F62 = Z13
F63 = Z13
F64 = Z13
F65 = Z11
F66 = Z11
F67 = Z13
F71 = Z33
F72 = Z33
F73 = Z33
F74 = Z33
F75 = Z31
F76 = Z31
F77 = Z33
F_c = BlockMatrix([[F11, F12, F13, F14, F15, F16, F17],
[F21, F22, F23, F24, F25, F26, F27],
[F31, F32, F33, F34, F35, F36, F37],
[F41, F42, F43, F44, F45, F46, F47],
[F51, F52, F53, F54, F55, F56, F57],
[F61, F62, F63, F64, F65, F66, F67],
[F71, F72, F73, F74, F75, F76, F77]])
print(F_c)
I_d = Identity(17)
Delta_t = symbols('Delta_t')
F_k = block_collapse(I_d + F_c * Delta_t)
print(F_k)
# k=6
G11 = Z33
G12 = Z33
G13 = Z33
G14 = Z33
G15 = Z33
G16 = Z33
G17 = Z33
G18 = Z33
GZ3 = BlockMatrix([[Z33, Z33, Z33, Z33, Z33, Z33, Z33, Z33]])
G21 = MatrixSymbol('G21', 3, 3)
G22 = MatrixSymbol('G22', 3, 3)
G23 = MatrixSymbol('G23', 3, 3)
G24 = MatrixSymbol('G24', 3, 3)
G25 = MatrixSymbol('G25', 3, 3)
G26 = MatrixSymbol('G26', 3, 3)
G27 = I33
G28 = Z33
G41 = MatrixSymbol('G41', 3, 3)
G42 = MatrixSymbol('G42', 3, 3)
G43 = MatrixSymbol('G43', 3, 3)
G44 = MatrixSymbol('G44', 3, 3)
G45 = MatrixSymbol('G45', 3, 3)
G46 = MatrixSymbol('G46', 3, 3)
G47 = Z33
G48 = I33
G51 = Z13
G52 = Z13
G53 = Z13
G54 = Z13
G55 = Z13
G56 = Z13
G57 = Z13
G58 = Z13
G_c = BlockMatrix([[G11, G12, G13, G14, G15, G16, G17, G18],
[G21, G22, G23, G24, G25, G26, G27, G28],
[G11, G12, G13, G14, G15, G16, G17, G18],
[G41, G42, G43, G44, G45, G46, G47, G48],
[G51, G52, G53, G54, G55, G56, G57, G58],
[G51, G52, G53, G54, G55, G56, G57, G58],
[G11, G12, G13, G14, G15, G16, G17, G18]])
print(G_c)
Q11 = MatrixSymbol('sigma2_T', 3, 3)
Q22 = Q11
Q33 = Q11
Q44 = Q11
Q55 = Q11
Q66 = Q11
Q77 = MatrixSymbol('sigma2_A', 3, 3)
Q88 = MatrixSymbol('sigma2_M', 3, 3)
Q_c = BlockDiagMatrix(Q11, Q22, Q33, Q44, Q55, Q66, Q77, Q88)
Q_k = block_collapse(G_c * Q_c * G_c.T)
Delta_t_2 = symbols('Delta_t_2')
Q_k = block_collapse(Q_k * Delta_t_2)
print(simplify(Q_k))
# Discussion
P_p = MatrixSymbol('P_p', 3, 3)
P_v = MatrixSymbol('P_v', 3, 3)
P_q = MatrixSymbol('P_q', 3, 3)
P_a = MatrixSymbol('P_a', 3, 3)
P_t = MatrixSymbol('P_t', 1, 1)
P_m = MatrixSymbol('P_m', 1, 1)
P_j = MatrixSymbol('P_j', 3, 3)
P0 = BlockDiagMatrix(P_p, P_v, P_q, P_a, P_t, P_m, P_j)
F_kd = block_collapse(F_k - F_c * Delta_t + F_c)
Q_kd = block_collapse(G_c * Q_c * G_c.T)
P1 = block_collapse(F_kd * P0 * F_kd.T + Q_kd)
#print(P1)
P2 = block_collapse(F_kd * P1 * F_kd.T + Q_kd)
print(P2)
H = BlockMatrix([[I33, Z33, Z33, Z33, Z31, Z31, Z33],
[Z33, Z33, I33, Z33, Z31, Z31, Z33]])
S2 = block_collapse(H * P2 * H.T + Identity(6))
#print(S2)
K2 = block_collapse(P2 * H.T * S2.I)
#print(K2)
P2_upd = block_collapse((Identity(17) - K2 * H) * P2)
print(P2_upd)
