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