c4dynamics 2.0.3__py3-none-any.whl

c4dynamics/rotmat/rotmat.py

@@ -0,0 +1,351 @@
+import numpy as np
+import sys 
+sys.path.append('.')
+# import c4dynamics as c4d 
+from c4dynamics.utils.const import *  
+from c4dynamics.utils.math import *  
+
+
+def rotx(phi):
+    ''' 
+    Generate a 3x3 Direction Cosine Matrix for 
+    a positive rotation about the x-axis by an angle :math:`\\phi` in radians.
+
+    A right-hand rotation matrix about `x` is given by: 
+
+    .. math:: 
+        
+        R = \\begin{bmatrix}
+            1 & 0 & 0 \\\\ 
+                0 & cos(\\varphi) & sin(\\varphi) \\\\ 
+                    0 & -sin(\\varphi) & cos(\\varphi) 
+            \\end{bmatrix} 
+
+        
+
+    Parameters
+    ----------
+    phi : float or int
+        The angle of rotation in radians.
+
+    Returns
+    -------
+    out : numpy.array
+        A 3x3 rotation matrix representing the rotation about the x-axis.
+
+    
+    Examples
+    --------
+    
+    >>> rotx(0)  # doctest: +NUMPY_FORMAT
+    [[1  0  0]
+     [0  1  0]
+     [0  0  1]]
+
+    
+    >>> rotx(c4d.pi / 2)  # doctest: +NUMPY_FORMAT
+    [[1  0  0]
+     [0  0  1]
+     [0 -1  0]]
+
+           
+    >>> v1 = [0, 0, 1]
+    >>> phi = 90 * c4d.d2r
+    >>> rotx(phi) @ v1 # doctest: +NUMPY_FORMAT
+    [0  1  0]
+
+    
+    >>> phi = 45 * c4d.d2r
+    >>> rotx(phi) @ v1 # doctest: +NUMPY_FORMAT
+    [0  0.707  0.707]
+
+    '''
+    return np.array([[1, 0, 0], [0, cos(phi), sin(phi)], [0, -sin(phi), cos(phi)]])
+
+
+def roty(theta):
+    ''' 
+    Generate a 3x3 Direction Cosine Matrix for 
+    a positive rotation about the y-axis by an angle :math:`\\theta` in radians.
+
+
+
+    A right-hand rotation matrix about `y` is given by: 
+
+    .. math:: 
+        
+        R = \\begin{bmatrix}
+            cos(\\theta) & 0& -sin(\\theta) \\\\
+                0 & 1 & 0 \\\\ 
+                    sin(\\theta) & 0 & cos(\\theta)
+            \\end{bmatrix}  
+
+    
+    Parameters
+    ----------
+    theta : float or int
+        The angle of rotation in radians.
+
+    Returns
+    -------
+    out : numpy.array
+        A 3x3 rotation matrix representing the rotation about the y-axis.
+
+    Examples
+    --------
+    
+    >>> roty(0)  # doctest: +NUMPY_FORMAT
+    [[1  0  0]
+     [0  1  0]
+     [0  0  1]]
+           
+        
+    >>> roty(c4d.pi / 2) # doctest: +NUMPY_FORMAT
+    [[0  0 -1]
+     [0  1  0]
+     [1  0  0]]
+
+    
+    >>> v1 = [0, 0, 1]
+    >>> phi = 90 * c4d.d2r
+    >>> roty(phi) @ v1  # doctest: +NUMPY_FORMAT
+    [-1  0  0]
+
+    
+    >>> phi = 45 * c4d.d2r
+    >>> roty(phi) @ v1 # doctest: +NUMPY_FORMAT
+    [-0.707  0  0.707]
+
+    '''
+    return np.array([[cos(theta), 0, -sin(theta)], [0, 1, 0], [sin(theta), 0, cos(theta)]])
+
+
+def rotz(psi):
+    ''' 
+    Generate a 3x3 Direction Cosine Matrix for 
+    a positive rotation about the z-axis by an angle :math:`\\psi` in radians.
+
+    A right-hand rotation matrix about `y` is given by: 
+
+
+    .. math:: 
+        
+        R = \\begin{bmatrix}
+            cos(\\psi) & sin(\\psi) & 0 \\\\
+                -sin(\\psi) & cos(\\psi) & 0 \\\\ 
+                    0 & 0 & 1
+            \\end{bmatrix}  
+
+
+
+    Parameters
+    ----------
+    psi : float or int
+        The angle of rotation in radians.
+
+    Returns
+    -------
+    out : numpy.array
+        A 3x3 rotation matrix representing the rotation about the z-axis.
+
+    Examples
+    --------
+    
+    >>> rotz(0)  # doctest: +NUMPY_FORMAT
+    [[1  0  0]
+     [0  1  0]
+     [0  0  1]]
+
+
+    >>> rotz(c4d.pi / 2)  # doctest: +NUMPY_FORMAT
+    [[0   1  0]
+     [-1  0  0]
+     [0   0  1]]
+
+    
+    >>> v1 = [0.707, 0.707, 0]
+    >>> phi = 90 * c4d.d2r
+    >>> rotz(phi) @ v1 # doctest: +NUMPY_FORMAT
+    [0.707  -0.707  0]
+
+    >>> phi = 45 * c4d.d2r
+    >>> rotz(phi) @ v1 # doctest: +NUMPY_FORMAT
+    [1  0  0]
+    
+    '''
+    return np.array([[cos(psi), sin(psi), 0], [-sin(psi), cos(psi), 0], [0, 0, 1]])
+
+
+def dcm321(phi = 0.0, theta = 0.0, psi = 0.0):
+    '''
+    Generate a 3x3 Direction Cosine Matrix (DCM) for a sequence of 
+    positive rotations around the axes in the following order: 
+    :math:`z`, then :math:`y`, then :math:`x`.
+
+    The final form of the matrix is given by: 
+
+    .. math:: 
+        
+        R = \\begin{bmatrix}
+              c\\theta \\cdot c\\psi 
+            & c\\theta \\cdot s\\psi 
+            & -s\\theta \\\\
+                  s\\varphi \\cdot s\\theta \\cdot c\\psi - c\\varphi \\cdot s\\psi 
+                & s\\varphi \\cdot s\\theta \\cdot s\\psi + c\\varphi \\cdot c\\psi 
+                & s\\varphi \\cdot c\\theta \\\\ 
+                      c\\varphi \\cdot s\\theta \\cdot c\\psi + s\\varphi \\cdot s\\psi 
+                    & c\\varphi \\cdot s\\theta \\cdot s\\psi - s\\varphi \\cdot c\\psi 
+                    & c\\varphi \\cdot c\\theta
+            \\end{bmatrix}  
+
+    where 
+
+    - :math:`c\\varphi \\equiv cos(\\varphi)`
+    - :math:`s\\varphi \\equiv sin(\\varphi)`
+    - :math:`c\\theta \\equiv cos(\\theta)`
+    - :math:`s\\theta \\equiv sin(\\theta)`
+    - :math:`c\\psi \\equiv cos(\\psi)`
+    - :math:`s\\psi \\equiv sin(\\psi)`    
+
+
+
+    Parameters
+    ----------
+    phi : float or int 
+        The angle in radian of rotation about `x`, default :math:`\\phi = 0`. 
+    theta : float or int 
+        The angle in radian of rotation about `y`, default :math:`\\theta = 0`. 
+    psi : float or int 
+        The angle in radian of rotation about `z`, default :math:`\\psi = 0`. 
+
+    Returns
+    -------
+    out : numpy.array
+        3x3 Direction Cosine Matrix representing the combined rotation.
+
+        
+    Examples
+    --------
+
+    The inertial velocity vector of an aircraft expressed in an inertial earth frame is given by:
+    
+    >>> v = [150, 0, 0]
+    
+    The attitude of the aircraft with respect to the inertial earth frame is
+    given by the 3 Euler angles: 
+
+    .. math:: 
+
+        \\phi = 0
+        
+        \\theta = 30 \\cdot {\\pi \\over 180}
+        
+        \\psi = 0
+            
+    The velcoty expressed in body frame:
+    
+    >>> dcm321(phi = 0, theta = 30 * c4d.d2r, psi = 0) @ v  # doctest: +NUMPY_FORMAT
+    [129.9  0  75]
+
+    '''
+    return rotx(phi) @ roty(theta) @ rotz(psi)
+
+
+def dcm321euler(dcm):
+    '''    
+    Extract Euler angles (roll, pitch, yaw) from a Direction Cosine Matrix (DCM) of 3-2-1 order.
+
+    The form of a 3-2-1 rotation matrix:   
+
+    .. math:: 
+
+        R = \\begin{bmatrix}
+            c\\theta \\cdot c\\psi 
+            & c\\theta \\cdot s\\psi 
+            & -s\\theta \\\\
+                s\\varphi \\cdot s\\theta \\cdot c\\psi - c\\varphi \\cdot s\\psi 
+                & s\\varphi \\cdot s\\theta \\cdot s\\psi - c\\varphi \\cdot c\\psi 
+                & s\\varphi \\cdot c\\theta \\\\ 
+                    s\\varphi \\cdot s\\theta \\cdot s\\psi + s\\varphi \\cdot s\\psi 
+                    & s\\varphi \\cdot s\\theta \\cdot s\\psi - s\\varphi \\cdot c\\psi 
+                    & c\\varphi \\cdot c\\theta
+            \\end{bmatrix}  
+
+    where 
+
+    - :math:`c\\varphi \\equiv cos(\\varphi)`
+    - :math:`s\\varphi \\equiv sin(\\varphi)`
+    - :math:`c\\theta \\equiv cos(\\theta)`
+    - :math:`s\\theta \\equiv sin(\\theta)`
+    - :math:`c\\psi \\equiv cos(\\psi)`
+    - :math:`s\\psi \\equiv sin(\\psi)`
+        
+
+    Parameters
+    ----------
+    dcm : numpy.array
+        3x3 Direction Cosine Matrix representing a rotation.
+
+    Returns
+    -------
+    out : tuple
+        A tuple containing Euler angles (yaw, pitch, roll) in degrees.
+
+    Notes
+    -----
+    Each set of Euler angles has a geometric singularity where 
+    two angles are not uniquely defined.
+    It is always the second angle which defines this singular orientation: 
+
+    - Symmetric Set: 2nd angle is 0 or 180 degrees. For example the 3-1-3 orbit angles with zero inclination.
+    - Asymmetric Set: 2nd angle is ±90 degrees. For example, the 3-2-1 angle of an aircraft with 90 degrees pitch. 
+
+    Examples
+    --------
+
+    >>> dcm321euler(np.eye(3)) # doctest: +NUMPY_FORMAT
+    (0, 0, 0)    
+        
+    A rotation matrix that represents the attitude of an aircraft with respect to 
+    an inertial earth frame is given by:
+
+    >>> BI = np.array([[ 0.866,     0, -0.5      ]
+    ...                 , [ 0,      1,  0        ]
+    ...                 , [ 0.5,    0,  0.866    ]])
+    >>> dcm321euler(BI) # doctest: +NUMPY_FORMAT
+    (0, 30, 0)
+
+    '''
+    
+    psi   =  atan2(dcm[0, 1], dcm[0, 0]) * r2d
+    theta = -asin(dcm[0, 2]) * r2d
+    phi   =  atan2(dcm[1, 2], dcm[2, 2]) * r2d
+
+    return phi, theta, psi 
+
+
+
+if __name__ == "__main__":
+
+  import doctest, contextlib, os
+  from c4dynamics import IgnoreOutputChecker, cprint
+  
+  # Register the custom OutputChecker
+  doctest.OutputChecker = IgnoreOutputChecker
+
+  tofile = False 
+  optionflags = doctest.FAIL_FAST
+
+  if tofile: 
+    with open(os.path.join('tests', '_out', 'output.txt'), 'w') as f:
+      with contextlib.redirect_stdout(f), contextlib.redirect_stderr(f):
+        result = doctest.testmod(optionflags = optionflags) 
+  else: 
+    result = doctest.testmod(optionflags = optionflags)
+
+  if result.failed == 0:
+    cprint(os.path.basename(__file__) + ": all tests passed!", 'g')
+  else:
+    print(f"{result.failed}")
+
+

c4dynamics/sensors/__init__.py

@@ -0,0 +1,72 @@
+'''
+
+`c4dynamics` provides sensor models to simulate real world applications.   
+The models include the functionality and the errors model 
+of electro-optic, lasers, and electro-magnetic devices. 
+
+'''
+#
+# i think maybe to include also detectors in this module
+# and rename it to something like perception \ source \ input 
+# measures \ 
+# 
+
+import sys 
+sys.path.append('.')
+
+from c4dynamics.sensors.radar import radar
+from c4dynamics.sensors.lineofsight import lineofsight 
+from c4dynamics.sensors.seeker import seeker
+
+
+# Background Material
+# -------------------
+
+# Introduction
+# ^^^^^^^^^^^^
+# '''
+
+# seekers:
+#   matter:
+# 	    radar
+# 	    laser
+# 	    optic
+#   functionallity:
+#       altitude radar 
+#       lineofsight seeker
+	
+# sensors:
+# 	imu 
+# 		accelerometers
+# 		roll gyro
+# 		rate gyro
+# 	gps
+# 	lidar
+
+
+
+if __name__ == "__main__":
+
+  import doctest, contextlib, os
+  from c4dynamics import IgnoreOutputChecker, cprint
+  
+  # Register the custom OutputChecker
+  doctest.OutputChecker = IgnoreOutputChecker
+
+  tofile = False 
+  optionflags = doctest.FAIL_FAST
+
+  if tofile: 
+    with open(os.path.join('tests', '_out', 'output.txt'), 'w') as f:
+      with contextlib.redirect_stdout(f), contextlib.redirect_stderr(f):
+        result = doctest.testmod(optionflags = optionflags) 
+  else: 
+    result = doctest.testmod(optionflags = optionflags)
+
+  if result.failed == 0:
+    cprint(os.path.basename(__file__) + ": all tests passed!", 'g')
+  else:
+    print(f"{result.failed}")
+
+
+

c4dynamics/sensors/lineofsight.py

@@ -0,0 +1,78 @@
+import numpy as np
+
+import c4dynamics as c4d 
+
+# np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)                 
+
+class lineofsight:
+  ''' 
+  Line of Sight seeker. 
+  
+  the lineofsight is measure line of sight vector rate 
+  the seeker head lags the true angular rate. 
+  this lag is represented by a first order transfer function with time constant tau1 
+  there are assumed to be delays involved in processing the seeker head angular rate singal. 
+  the filter delays are represented by a first order transfer function with time constant tau2
+
+  Parameters
+  ==========
+  TODO complete 
+
+  Example
+  =======
+  TODO complete
+
+  '''
+  
+  tau1 = 0
+  tau2 = 0
+  dt = 0
+  
+  isideal = False 
+  
+  
+  omega = np.array([0, 0, 0])     # truth los rate
+  omega_ach = np.array([0, 0, 0]) # achieved los rate after first order filter
+  omega_f = np.array([0, 0, 0])     # filtered los rate after first order filter 
+  
+  _data = np.zeros((1, 7))
+    
+  def __init__(obj, dt, tau1 = 0.05, tau2 = 0.05, isideal = False): #**kwargs):
+    # obj.__dict__.update(kwargs)
+    obj.dt = dt
+    obj.tau1 = tau1 # tracking loop time constant 
+    obj.tau2 = tau2 # seeker signal processing time constant  
+    obj.isideal = isideal 
+    
+  
+  def measure(obj, r, v):
+    # r: range to target (line of sight vector)
+    # v: relative velocity with target 
+
+    # 
+    # true angular rate of the los vector  
+    ##
+    obj.omega = np.cross(r, v) / np.linalg.norm(r)**2 
+    
+    # 
+    # achieved seeker-head angular rate vector
+    ##
+    obj.omega_ach = obj.omega_ach * np.exp(-obj.dt / obj.tau1) + obj.omega * (1 - np.exp(-obj.dt / obj.tau1)) # lag 
+    
+    # 
+    # final processed tracking rate signal vector 
+    ## 
+    obj.omega_f = obj.omega_f * np.exp(-obj.dt / obj.tau2) + obj.omega_ach * (1 - np.exp(-obj.dt / obj.tau2)) # filter 
+    
+    if obj.isideal:
+      obj.omega_f = obj.omega_ach = obj.omega
+      
+      
+    return obj.omega_f
+
+  
+  def store(obj, t = -1):
+    obj._data = np.vstack((obj._data
+                           , np.concatenate(([t], obj.omega, obj.omega_f)))).copy()
+
+