```python
from sympy import *
from barfoot_utils_np import create_rot
init_printing()
```


```python
# State
x, y, z, rho, phi, theta = symbols('x y z \\rho \\phi \\theta', real=True)
xn, yn, zn, rhon, phin, thetan = symbols('xn yn zn rhon phin thetan', real=True)

# Landmark
mx, my, mz = symbols('mx my mz', real=True)

# Control input
vx, vy, vz, wx, wy, wz = symbols('v_x v_y v_z \\omega_x \\omega_y \\omega_z', real=True)
dt = Symbol('dt', real=True)

# Vehicle state and landmark
X = Matrix([x,y,z, rho, phi, theta])
Xn = Matrix([xn,yn,zn, rhon, phin, thetan])
V = Matrix([vx,vy,vz, wx, wy, wz])
m = Matrix([mx, my, mz])
```


```python
def create_rot_sym(Xrot):

    '''
    :param Xrot: Euler angles (1x3)
    '''

    # each dimension
    Rx = rot_axis1(Xrot[0])
    Ry = rot_axis2(Xrot[1])
    Rz = rot_axis3(Xrot[2])
    
    # Sympy doesn't follow the right-hand rule
    Rz[0,1] = -Rz[0,1]
    Rz[1,0] = -Rz[1,0]
    Ry[0,2] = -Ry[0,2]
    Ry[2,0] = -Ry[2,0]
    Rx[1,2] = -Rx[1,2]
    Rx[2,1] = -Rx[2,1]

    Rxyz = Rz.multiply(Ry).multiply(Rx)

    return Rxyz
```


```python
def dynamics(X, V):

    '''
    :param X: latent state (1x6)
    :param V: control input (1x6)
    '''

    dV = Matrix([V[3:6]])
    Rxyz = create_rot_sym(X[3:6])
    dX = Rxyz.subs([
        (rho, X[3]),
        (phi, X[4]),
        (theta, X[5])
    ]).multiply(Matrix(V[0:3]))
    return Matrix([*dX, *dV])

dynamics(X, V)
```




$\displaystyle \left[\begin{matrix}v_{x} \cos{\left(\phi \right)} \cos{\left(\theta \right)} + v_{y} \left(\sin{\left(\phi \right)} \sin{\left(\rho \right)} \cos{\left(\theta \right)} - \sin{\left(\theta \right)} \cos{\left(\rho \right)}\right) + v_{z} \left(\sin{\left(\phi \right)} \cos{\left(\rho \right)} \cos{\left(\theta \right)} + \sin{\left(\rho \right)} \sin{\left(\theta \right)}\right)\\v_{x} \sin{\left(\theta \right)} \cos{\left(\phi \right)} + v_{y} \left(\sin{\left(\phi \right)} \sin{\left(\rho \right)} \sin{\left(\theta \right)} + \cos{\left(\rho \right)} \cos{\left(\theta \right)}\right) + v_{z} \left(\sin{\left(\phi \right)} \sin{\left(\theta \right)} \cos{\left(\rho \right)} - \sin{\left(\rho \right)} \cos{\left(\theta \right)}\right)\\- v_{x} \sin{\left(\phi \right)} + v_{y} \sin{\left(\rho \right)} \cos{\left(\phi \right)} + v_{z} \cos{\left(\phi \right)} \cos{\left(\rho \right)}\\\omega_x\\\omega_y\\\omega_z\end{matrix}\right]$




```python
def measurment_model(X, L):

    '''
    :param X: latent state (1x6)
    :param L: landmarks
    '''

    T = eye(4)
    T[0:3, 0:3] = create_rot_sym(X[3:6])
    T[0:3, 3] = X[0:3]
    Tinv = eye(4)
    Tinv[0:3, 0:3] = T[0:3, 0:3].T
    Tinv[0:3, 3] = -T_inv[0:3,0:3]*T[0:3,3]

    return Tinv

measurment_model(X, None)
```




$\displaystyle \left[\begin{matrix}\cos{\left(\phi \right)} \cos{\left(\theta \right)} & \sin{\left(\theta \right)} \cos{\left(\phi \right)} & - \sin{\left(\phi \right)} & - x\\\sin{\left(\phi \right)} \sin{\left(\rho \right)} \cos{\left(\theta \right)} - \sin{\left(\theta \right)} \cos{\left(\rho \right)} & \sin{\left(\phi \right)} \sin{\left(\rho \right)} \sin{\left(\theta \right)} + \cos{\left(\rho \right)} \cos{\left(\theta \right)} & \sin{\left(\rho \right)} \cos{\left(\phi \right)} & - y\\\sin{\left(\phi \right)} \cos{\left(\rho \right)} \cos{\left(\theta \right)} + \sin{\left(\rho \right)} \sin{\left(\theta \right)} & \sin{\left(\phi \right)} \sin{\left(\theta \right)} \cos{\left(\rho \right)} - \sin{\left(\rho \right)} \cos{\left(\theta \right)} & \cos{\left(\phi \right)} \cos{\left(\rho \right)} & - z\\0 & 0 & 0 & 1\end{matrix}\right]$



1d205b5b11d6296e9eb1da184876709e22f8e4e4
ipynb
Jupyter Notebook
examples/dynamics.ipynb
cisprague/GPt
5aeccfedcfa5d72ba4596126956274f69e568535
Apache-2.0
examples/dynamics.ipynb
cisprague/GPt
5aeccfedcfa5d72ba4596126956274f69e568535
Apache-2.0
examples/dynamics.ipynb
cisprague/GPt
5aeccfedcfa5d72ba4596126956274f69e568535
Apache-2.0
Qwen/Qwen-72B
1. YES
2. YES