def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(
1) # Recycle these common nodes as much as possible.
for variable_list in [
self.states, self.alg_states, self.inputs, self.parameters,
self.constants
]:
attribute_lists = [[] for i in range(len(ast.Symbol.ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(
ast.Symbol.ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(
value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[
ca.veccat(*attribute_list)
for attribute_list in attribute_lists
])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
contains_if_else = ca.OP_IF_ELSE_ZERO in [
f.instruction_id(k) for k in range(f.n_instructions())
]
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var),
in_var).is_zero()
if contains_if_else or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return ca.Function('variable_metadata', [in_var], out)
def variable_metadata_function(self):
in_var = ca.veccat(*self._symbols(self.parameters))
out = []
is_affine = True
zero, one = ca.MX(0), ca.MX(1) # Recycle these common nodes as much as possible.
for variable_list in [self.states, self.alg_states, self.inputs, self.parameters, self.constants]:
attribute_lists = [[] for i in range(len(CASADI_ATTRIBUTES))]
for variable in variable_list:
for attribute_list_index, attribute in enumerate(CASADI_ATTRIBUTES):
value = ca.MX(getattr(variable, attribute))
if value.is_zero():
value = zero
elif value.is_one():
value = one
value = value if value.numel() != 1 else ca.repmat(value, *variable.symbol.size())
attribute_lists[attribute_list_index].append(value)
expr = ca.horzcat(*[ca.veccat(*attribute_list) for attribute_list in attribute_lists])
if len(self.parameters) > 0 and isinstance(expr, ca.MX):
f = ca.Function('f', [in_var], [expr])
# NOTE: This is not a complete list of operations that can be
# handled in an affine expression. That said, it should
# capture the most common ways variable attributes are
# expressed as a function of parameters.
allowed_ops = {ca.OP_INPUT, ca.OP_OUTPUT, ca.OP_CONST,
ca.OP_SUB, ca.OP_ADD, ca.OP_SUB, ca.OP_MUL, ca.OP_DIV, ca.OP_NEG}
f_ops = {f.instruction_id(k) for k in range(f.n_instructions())}
contains_unallowed_ops = not f_ops.issubset(allowed_ops)
zero_hessian = ca.jacobian(ca.jacobian(expr, in_var), in_var).is_zero()
if contains_unallowed_ops or not zero_hessian:
is_affine = False
out.append(expr)
if len(self.parameters) > 0 and is_affine:
# Rebuild variable metadata as a single affine expression, if all
# subexpressions are affine.
in_var_ = ca.MX.sym('in_var', in_var.shape)
out_ = []
for o in out:
Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)])
bf = ca.Function('bf', [in_var], [o])
A = Af(0)
A = ca.sparsify(A)
b = bf(0)
b = ca.sparsify(b)
o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b
out_.append(o_)
out = out_
in_var = in_var_
return self._expand_mx_func(ca.Function('variable_metadata', [in_var], out))
def sqrt_correct(Rs, H, W):
"""
source: Fast Stable Kalman Filter Algorithms Utilising the Square Root, Steward 98
Rs: sqrt(R)
H: measurement matrix
W: sqrt(P)
@return:
Wp: sqrt(P+) = sqrt((I - KH)P)
K: Kalman gain
Ss: Innovation variance
"""
n_x = H.shape[1]
n_y = H.shape[0]
B = ca.sparsify(ca.blockcat(Rs, ca.mtimes(H, W), ca.SX.zeros(n_x, n_y), W))
# qr by default is upper triangular, so we transpose inputs and outputs
B_Q, B_R = ca.qr(B.T) # B_Q orthogonal, B_R, lower triangular
B_Q = B_Q.T
B_R = B_R.T
Wp = B_R[n_y:, n_y:]
Ss = B_R[:n_y, :n_y]
P_HT_SsInv = B_R[n_y:, :n_y]
K = ca.mtimes(P_HT_SsInv, ca.inv(Ss))
return Wp, K, Ss
def sqrt_covariance_predict(W, F, Q):
"""
Finds a sqrt factorization of the continuous time covariance
propagation equations. Requires solving a linear system of equations
to keep the sqrt lower triangular.
'A Square Root Formulation of the Kalman Covariance Equations', Andrews 68
W: sqrt P, symbolic, with sparsity lower triangulr
F: dynamics matrix
Q: process noise matrix
returns:
W_dot_sol: sqrt of P deriative, lower triangular
"""
n_x = F.shape[0]
XL = ca.SX.sym('X', ca.Sparsity_lower(n_x))
X = (XL - XL.T)
for i in range(n_x):
X[i, i] = 0
W_dot = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X, ca.inv(W).T)
# solve for XI that keeps W dot lower triangular
y = ca.vertcat(*ca.triu(W_dot, False).nonzeros())
x_dep = []
for i, xi in enumerate(XL.nonzeros()):
if ca.depends_on(y, xi):
x_dep += [xi]
x_dep = ca.vertcat(*x_dep)
A = ca.jacobian(y, x_dep)
for i, xi in enumerate(XL.nonzeros()):
assert not ca.depends_on(A, xi)
b = -ca.substitute(y, x_dep, 0)
x_sol = ca.solve(A, b)
X_sol = ca.SX(X)
for i in range(x_dep.shape[0]):
X_sol = ca.substitute(X_sol, x_dep[i], x_sol[i])
X_sol = ca.sparsify(X_sol)
W_dot_sol = ca.mtimes(F, W) + ca.mtimes(Q / 2 + X_sol, ca.inv(W).T)
return W_dot_sol