ekf.py

q0, q1, q2, q3 = symbols("q_0 q_1 q_2 q_3")
q = Quaternion(q0, q1, q2, q3)
bx = symbols("b_x")
by = symbols("b_y")
bz = symbols("b_z")
x = Matrix([q0, q1, q2, q3, bx, by, bz])

q0_next, q1_next, q2_next, q3_next = symbols("q0_next q1_next q2_next q3_next")
q_next = Quaternion(q0_next, q1_next, q2_next, q3_next)
bx_next = symbols("bx_next")
by_next = symbols("by_next")
bz_next = symbols("bz_next")
x_next = Matrix([q0_next, q1_next, q2_next, q3_next, bx_next, by_next, bz_next])

wx, wy, wz, dt = symbols("w_x, w_y, w_z, dt")

w = Matrix([wx, wy, wz])
b = Matrix([bx, by, bz])

wq = Quaternion(0, wx, wy, wz)
bq = Quaternion(0, bx, by, bz)

A = MatrixSymbol('A', 7, 7)
B = MatrixSymbol('B', 7, 3)
    
linsolve([
         q_next - (dt / 2.0) * q * wq + (dt / 2.0) * q * bq - q,
         x_next - A * x - B * w,
         bx_next - bx,
         by_next - by,
         bz_next - bz,
        ], [A, B])