c4dynamics 2.0.3__py3-none-any.whl

c4dynamics/filters/lowpass.py

@@ -0,0 +1,123 @@
+import numpy as np
+import sys
+from typing import Optional
+
+sys.path.append('.')
+import c4dynamics as c4d
+
+
+class lowpass(c4d.state):
+  """
+    A first-order low-pass filter for smoothing signals, supporting both discrete and continuous systems.
+
+    Parameters
+    ----------
+    alpha : float, optional
+        Smoothing factor for a discrete system. Must be in the range (0, 1). Defaults to None.
+    dt : float, optional
+        Time step for a continuous system. Must be positive. Defaults to None.
+    tau : float, optional
+        Time constant for a continuous system. Must be positive. Defaults to None.
+    y0 : float, optional
+        Initial value of the state. Defaults to 0.
+
+    Raises
+    ------
+    ValueError
+        If neither `alpha` nor both `dt` and `tau` are provided.
+        If `alpha` is out of the range (0, 1) for a discrete system.
+        If `dt` or `tau` is non-positive for a continuous system.
+
+    Notes
+    -----
+    - For a continuous system, `dt` and `tau` are required, and `alpha` is calculated as `dt / tau`.
+    - For a discrete system, `alpha` alone is required and directly specifies the smoothing factor.
+
+    Example
+    -------
+
+    .. code:: 
+
+      >>> filter_continuous = lowpass(dt=0.01, tau=0.1, y0=0)
+      >>> filter_discrete = lowpass(alpha=0.5, y0=1)
+      >>> filter_continuous.sample(1.0) # doctest: +ELLIPSIS
+      0.09...
+      >>> filter_discrete.sample(2.0)
+      1.5
+  """
+
+  def __init__(self, alpha: Optional[float] = None, dt: Optional[float] = None,
+                  tau: Optional[float] = None, y0: float = 0) -> None:
+    # Initialize alpha based on the provided parameters
+    if dt is not None and tau is not None:
+      if dt <= 0 or tau <= 0:
+        raise ValueError("For a continuous system, `dt` and `tau` must be positive.")
+      self.alpha = dt / tau
+    elif alpha is not None:
+      if not (0 < alpha < 1):
+        raise ValueError("For a discrete system, `alpha` must be in the range (0, 1).")
+      self.alpha = alpha
+    else:
+      raise ValueError("Provide either `alpha` for a discrete system or both `dt` and `tau` for a continuous system.")
+
+    self.y = y0  # Initial state value
+
+  def sample(self, x: float) -> float:
+    """
+      Applies the low-pass filter to the input value and returns the filtered output.
+
+      Parameters
+      ----------
+      x : float
+          Input value to be filtered.
+
+      Returns
+      -------
+      float
+          The filtered output value after applying the low-pass filter.
+
+      Notes
+      -----
+      - For a continuous system: `y'(t) = -y(t) / tau + x(t) / tau`
+      - For a discrete system: `y[k] = (1 - alpha) * y[k-1] + alpha * x[k]`
+      - The filter's state (`self.y`) is updated in place.
+
+      Example
+      -------
+
+      .. code::
+
+        >>> lp_filter = lowpass(alpha=0.5)
+        >>> lp_filter.sample(2.0)
+        1.0
+        >>> lp_filter.sample(3.0)
+        2.0
+    """
+    # Update the filter's state
+    self.y = (1 - self.alpha) * self.y + self.alpha * x
+    return self.y
+
+
+if __name__ == "__main__":
+    import doctest
+    import contextlib
+    import os
+    from c4dynamics import IgnoreOutputChecker, cprint
+
+    # Register the custom OutputChecker
+    doctest.OutputChecker = IgnoreOutputChecker
+
+    tofile = False
+    optionflags = doctest.FAIL_FAST
+
+    if tofile:
+        with open('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} test(s) failed.")

c4dynamics/filters/luenberger.py

@@ -0,0 +1,97 @@
+import numpy as np
+
+class luenberger:
+  ''' 
+  Luenberger (Asymptotic) Observer.
+
+  
+  Parameters
+  ==========
+  TODO complete 
+
+  
+  luenberger estimator 
+  agranovich, 
+  modern control 72
+
+  ### Luenberger Filter
+  The Luenberger observer is given by:
+
+  \[ \dot{\hat{x}}(t) = A \hat{x}(t) + B u(t) + L (y(t) - C \hat{x}(t)) \]
+
+  where \( L \) is the observer gain matrix.
+
+  '''
+
+  A = 0
+  c = 0
+  obsv = 0  
+  Aest = 0
+
+  def __init__(obj, A, c): 
+    obj.A = A
+    obj.c = c
+    obj.obsv = np.copy(obj.c)
+    for n in range(len(obj.A) - 1):
+        obj.obsv = np.vstack((obj.obsv, obj.c @ obj.A**(n + 1))).copy()
+   
+  def isobservable(obj):
+    return np.linalg.matrix_rank(obj.obsv) == len(obj.A)
+        
+  def eig(obj):
+    return np.linalg.eig(obj.A)[0]
+  
+  def setest(obj, s):
+    n = len(obj.A)
+    
+    # coefficients from the desired eigenvalues
+    an_d = np.polynomial.polynomial.polyfromroots(s)
+    # the extended system eigenvalues are including the luenberger gains which are currently unknown.
+    # the polynomial that represents the system is given by the determinant of s*I-Aest. where Aest is the extended system matrix which inclueds the gains.
+    # the desired eigenvalues are given in the input argument s. 
+    # assuming the prime coefficient is one in the both systems, there are n-1 coefficients to compare.
+
+  
+    #
+    # the calculation of luenberger gains
+    #   rational:
+    #   1 calculate the luenberger gains of the equivalent canonical model
+    #   2 find the model transform matrix 
+    #   3 tranform the gains 
+    ##
+    
+    # a canonical equivalent system matrix:
+    #   | 0 0 ..  -a0 |
+    #   | 1 0 ..  -a1 |
+    #   | ..          |
+    #   |0 0 .. 1 -an-1|
+    #   where a0..an-1 are the coefficients of the system polynomial.
+    
+    an = np.polynomial.polynomial.polyfromroots(np.linalg.eig(obj.A)[0])
+    # canA = np.zeros((n, n))
+    # for i in range(n):
+    #   canA[i, -1] = -an[i]
+    #   if i == 0:
+    #     continue
+    #   canA[i, i - 1] = 1
+    
+    # luenberger gains for the canonical system
+    Lc = np.zeros(n) # Lcanonical 
+    for i in range(n):
+      Lc[i] = an_d[i] - an[i]
+    
+    # model transformation matrix
+    cmu = np.zeros(n)
+    cmu[-1] = 1
+    mu = np.linalg.solve(obj.obsv, cmu.reshape((-1, 1)))
+    
+    M = np.copy(mu)
+    for n in range(n - 1):
+        M = np.hstack((M, obj.A**(n + 1) @ mu)).copy()
+    
+    #   3 tranform the gains 
+    L = M @ Lc
+    
+    obj.Aest = obj.A - L @ obj.c
+    
+  

c4dynamics/rotmat/__init__.py

@@ -0,0 +1,141 @@
+'''
+
+Rotational Matrix Operations
+============================
+
+   
+
+Background Material 
+-------------------
+
+A rotation matrix is a mathematical representation of a 
+rotation in three-dimensional space. 
+
+It's a 3x3 matrix that, when multiplied with a vector, 
+transforms the vector to represent a new orientation. 
+
+Each element of the matrix corresponds to a directional cosine, 
+capturing the rotation's effect on the x, y, and z axes.
+
+
+Euler Angles Order
+^^^^^^^^^^^^^^^^^^
+
+Frame based vectors are related through a Direction Cosine Matrix (DCM). [HS]_
+
+When Euler angles are employed in the transformation of
+a vector expressed in one reference frame to the expression
+of the vector in a different reference frame, any order of the
+three Euler rotations is possible, but the resulting transformation
+equations depend on the order selected. [MIs]_ 
+
+In aerospace applications for example, 
+the common order is that the first Euler rotation is about the z-axis,
+the second is about the y-axis, and the third is about the Xaxis.
+Such a transformation order is called z-y-x, or 3-2-1. 
+With reference to a body orientation, the resulting
+order is yaw, pitch, and roll. 
+With reference to geographical
+orientation, the resulting order is azimuth (heading),
+elevation (pitch), and roll (bank angle).
+
+
+Right Hand Frame 
+^^^^^^^^^^^^^^^^
+
+The positive directions of coordinate system 
+axes and the directions of positive rotations 
+about the axes are arbitrary.
+In right-handed systems:
+
+:: 
+
+  i x j = k
+  j x k = i
+  k x i = j
+
+where i is the unit vector in the direction of the x-axis,
+j is the unit vector in the direction of the y-axis, 
+k is the unit vector in the direction of the z-axis.
+
+Positive rotations are clockwise
+when viewed from the origin, looking out along the
+positive direction of the axis. 
+
+These conventions are illustrated
+in Fig-1.
+
+
+.. figure:: /_architecture/frame_conventions.svg
+   
+   Fig-1: Coordinate System Conventions 
+
+
+References
+----------
+
+.. [HS] Hanspeter Schaub, "Spacecraft Dynamics and Control" lecture notes, module 2: rigidbody kinematics. 
+.. [MIs] Ch 4 in "Missile Flight Simulation Part One Surface-to-Air Missiles", Military Handbook, 1995, MIL-HDBK-1211(MI).    
+
+
+
+Examples
+--------
+
+For examples, see the various functions.
+
+''' 
+# spacecraft dyanmics and control 
+
+# Rotating Reference Frame
+# ^^^^^^^^^^^^^^^^^^^^^^^^
+
+# A vector resolved in a given reference frame is said to be 
+# **expressed** in that frame (sometimes said **referred to**). 
+
+# The rate of change of a vector, as viewed by an observer fixed to and moving 
+# with a given reference frame, 
+# is said to be **relative to** or **with respect to** that reference frame. 
+
+# It's important to note here that the rate of change of a vector must be 
+# relative to an inertial reference frame, 
+# but it can be expressed in any reference frame. 
+
+
+import sys 
+sys.path.append('.')
+
+from c4dynamics.rotmat.rotmat import rotx, roty, rotz, dcm321, dcm321euler
+from c4dynamics.rotmat.animate import animate  
+
+
+
+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}")
+
+
+
+
+
+
+