<div align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/utils/C4dynamics.png">
</div>
# Tsipor Dynamics
## Algorithms Engineering and Development
****
C4Dynamics (read Tsipor (bird) Dynamics) is the open-source framework of algorithms development for objects in space and time.
[](https://skillicons.dev)
## Table of contents
- [Motivation](https://github.com/C4dynamics/C4dynamics/tree/main/#motivation)
- [Installation](https://github.com/C4dynamics/C4dynamics/tree/main/#installation)
- [Quickstart](https://github.com/C4dynamics/C4dynamics/tree/main/#quickstart)
- [Architecture](https://github.com/C4dynamics/C4dynamics/tree/main/#architecture)
- [Contributors ✨](https://github.com/C4dynamics/C4dynamics/tree/main/#contributors-✨)
- [Quickstart for Contributors](https://github.com/C4dynamics/C4dynamics/tree/main/#quickstart-for-contributors)
- [Getting Started 1: Objects Detection and Tracking Example](https://github.com/C4dynamics/C4dynamics/tree/main/#getting-started-1-objects-detection-and-tracking-example)
- [Getting Started 2: Missile Guidance Example](https://github.com/C4dynamics/C4dynamics/tree/main/#getting-started-2-missile-guidance-example)
## Motivation
C4dynamics provides two basic entities for developing and analyzing algorithms of objects in space and time:
* datapoint: a class defining a point in space: position, velocity, acceleration, and mass.
* rigidbody: a class defining a rigid body in space, i.e. an object with length and angular position.
You can develop and analyze algorithms by operating on these objects with one of the internal systems or algorithms of C4dynamics:
* ODE Solver (4th order Runge-Kutta)
* Kalman Filter
* Extended Kalman Filter
* Luenberger Observer
* Radar System
* Altitude Radar
* IMU Model
* GPS Model
* Line Of Sight Seeker
Or one of the 3rd party libraries integrated with C4dynamics:
* NumPy
* Matplotlib
* OpenCV
* YOLO
Whether you're a seasoned algorithm engineer or just getting started, this framework has something to offer. Its modular design allows you to easily pick and choose the components you need, and its active community of contributors is always working to improve and expand its capabilities.
So why wait? Start using C4dynamics today and take your algorithms engineering to the next level!
## Installation
* Download C4dynamics: https://github.com/C4dynamics/C4dynamics
* Install the required packages:
```
pip install -r requirements.txt
```
* Alternatively, run the preinstalled conda environment (see conda_installation.md):
```
conda env create -f c4dynamics_env.yaml
```
## Quickstart
Import the framework:
```
import C4dynamics as c4d
```
Define a point in space with some initial conditions:
```
pt = c4d.datapoint(x = 1000, vx = 100)
```
Define a body in space with some initial conditions:
```
body = c4d.rigidbody(theta = 15 * 3.14 / 180)
```
Load an object detection module (YOLO):
```
yolodet = c4d.detectors.yolo(height = height, width = width)
```
Define a linear Kalman Filter, perform a prediction and an update:
```
pt.filter = c4d.filters.kalman(np.hstack((z, np.zeros(2))), P, A, H, Q, R)
pt.filter.predict()
pt.filter.correct(measure)
```
Store the current state of the datapoint (at time t):
```
pt.store(t)
```
Store other variables added to the datapoint object:
```
pt.storevar('kalman_state', t)
```
Define errors to a general-purpose seeker with C4dynamics:
```
rdr = c4d.seekers.radar(sf = 0.9, bias = 0, noisestd = 1)
```
## Architecture
For Architecture & Roadmap, see the Wiki page.
## Contributors ✨
[//]: contributor-faces
<a href="https://www.linkedin.com/in/ziv-meri/"> <img src="https://github.com/C4dynamics/C4dynamics/raw/main/utils/ziv_noa2.png" title="Ziv Meri" width="80" height="80"></a>
<a href="https://www.linkedin.com/in/aviva-shneor-simhon-17b733b/"> <img src="https://github.com/C4dynamics/C4dynamics/blob/main/utils/aviva2.png" title="Aviva Shneor Simhon" width="80" height="80"></a>
<a href="https://www.linkedin.com/in/amit-elbaz-54301382/"> <img src="https://github.com/C4dynamics/C4dynamics/blob/main/utils/amit2.png" title="Amit Elbaz" width="80" height="80"></a>
<a href="https://www.linkedin.com/in/avraham-ohana-computer-vision/"> <img src="https://github.com/C4dynamics/C4dynamics/blob/main/utils/avraham2.png" title="Avraham Ohana" width="80" height="80"></a>
<a href="https://chat.openai.com/chat"> <img src="https://github.com/C4dynamics/C4dynamics/blob/main/utils/openai-featured.png" title="Chat GPT" width="80" height="80"></a>
[//]: contributor-faces
This project follows the [all-contributors](https://github.com/all-contributors/all-contributors) specification. Contributions of any kind welcome!
## Quickstart for Contributors
* See the page contributing.md
* In any case, it's a good advise to start with the example dof6sim.ipynb and change the missile-target conditions to gain some experience with the framework. This example appears also down here in the README
## Getting Started 1: Objects Detection and Tracking Example
### Car Detection and Tracking with YOLO and Kalman Filter using C4dynamics
see the notebook: examples\cars_tracker.ipynb
This is an example of detecting vehicles in images using YOLO, and then tracking those detected cars using a Kalman filter.
For the demonstration, we will be using a dataset of traffic surveillance videos. The dataset contains video sequences recorded from a traffic camera, capturing various vehicles including cars.
#### Let's start!
* Load 3rd party modules:
```
import numpy as np
import cv2
from sklearn.neighbors import NearestNeighbors
from matplotlib import pyplot as plt
from matplotlib import image as mpimg
```
* Load the vidoes:
```
videoin = os.path.join(os.getcwd(), 'examples', 'cars1.mp4')
videoout = os.path.join(os.getcwd(), 'examples', 'out', 'cars1.mp4')
```
* Video preprocessing:
```
video = cv2.VideoCapture(videoin)
dt = 1 / video.get(cv2.CAP_PROP_FPS)
_, frame1 = video.read()
height, width, _ = frame1.shape
result = cv2.VideoWriter(videoout, cv2.VideoWriter_fourcc(*'mp4v')
, int(video.get(cv2.CAP_PROP_FPS)), [width, height])
```
```
plt.imshow(mpimg.imread(os.path.join(os.getcwd(), 'examples', 'out', 'before.png')))
plt.axis('off')
plt.show()
```
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars1.png?raw=true" width="325" height="260"/>
</p>
* Load a detector:
```
yolodet = c4d.detectors.yolo(height = height, width = width)
```
* Define Kalman parameters:
```
# x(t) = A(t) * x(t-1) + e(t) || e(t) ~ N(0,Q(t))
# z(t) = H(t) * x(t) + r(t) || r(t) ~ N(0,R(t))
# x(t) = [x1 y1 x2 y2 vx vy]
A = np.array([[1, 0, 0, 0, dt, 0 ]
, [0, 1, 0, 0, 0, dt]
, [0, 0, 1, 0, dt, 0 ]
, [0, 0, 0, 1, 0, dt]
, [0, 0, 0, 0, 1, 0 ]
, [0, 0, 0, 0, 0, 1 ]])
H = np.array([[1, 0, 0, 0, 0, 0]
, [0, 1, 0, 0, 0, 0]
, [0, 0, 1, 0, 0, 0]
, [0, 0, 0, 1, 0, 0]])
P = np.eye(A.shape[1])
Q = np.array([[25, 0, 0, 0, 0, 0]
, [0, 25, 0, 0, 0, 0]
, [0, 0, 25, 0, 0, 0]
, [0, 0, 0, 25, 0, 0]
, [0, 0, 0, 0, 49, 0]
, [0, 0, 0, 0, 0, 49]])
R = dt * np.eye(4)
```
* Define data-point object
Tracker = data-point + Kalman Filter
```
class tracker(c4d.datapoint):
# a datapoint
# kalman filter
# display color
##
def __init__(obj, z):
super().__init__() # Call the constructor of c4d.datapoint
obj.filter = c4d.filters.kalman(np.hstack((z, np.zeros(2))), P, A, H, Q, R)
obj.counterSame = 0
obj.counterEscaped = 0
obj.appear = 0
obj.color = [np.random.randint(0, 255)
, np.random.randint(0, 255)
, np.random.randint(0, 255)]
```
* Define trackers manager:
(a dictionary of trackers and methods to add and remove elements)
```
# trackers manager:
# list of active trackers
# add or remove tracks methods
class mTracks:
def __init__(obj):
obj.trackers = {}
obj.removelist = []
obj.neigh = NearestNeighbors(n_neighbors = 1)
obj.thresh = 100 # threshold of 100 pixels to verify the measure is close enough to the object.
```
* Main loop:
```
def run(tf = 1):
mtracks = mTracks()
cov = 25 * np.eye(4)
t = 0
while video.isOpened():
ret, frame = video.read()
# frame = frame.astype(np.uint8)
# frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) # Assuming input frames are in BGR format
if not ret:
break
#
# the update function runs the main trackers loop.
##
zList = yolodet.getMeasurements(frame) # retrieves object measurements from the current frame using the object detector (YoloDetector).
# updates the trackers dictionary by adding or removing tracks.
# creates new trackers if there are more
# measurements than trackers and removes duplicate trackers if
# there are more trackers than measurements.
kfNumber = len(mtracks.trackers.keys())
zNumber = zList.shape[0]
if kfNumber < zNumber:
# more measurements than trackers
mtracks.trackerMaker(zList, cov)
elif kfNumber > zNumber and zNumber > 0:
# more trackers than measurements
mtracks.RemoveDoubles()
# fits the Nearest Neighbors algorithm to the object
# measurements for association purposes.
if zList.shape[0] >= 2:
mtracks.neigh.fit(zList)
mtracks.refresh(zList, frame, t)
mtracks.remove()
cv2.imshow('image', frame)
result.write(frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q') or t >= tf:
break
t += dt
cv2.destroyAllWindows()
result.release()
return mtracks
```
* Run for two seconds:
```
ltrk = run(tf = 2)
```
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_objected_tracked.gif?raw=true" width="325" height="260"/>
</p>
* Results analysis:
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_id.png?raw=true" width="325" height="260"/>
</p>
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_trajectories.png?raw=true" width="325" height="260"/>
</p>
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_velocity.png?raw=true" width="325" height="260"/>
</p>
## Getting Started 2: Missile Guidance Example
### 6 DOF simulation of a missile employing proportional navigation guidance to pursue a target.
#### Let's start:
* Load third party modules
```
import sys, os
import numpy as np
from matplotlib import pyplot as plt
```
* Add C4dynamics to the python path and load it
```
sys.path.append(os.path.join(os.getcwd(), '..', '..'))
import C4dynamics as c4d
```
* Load other modules that developed for the demonstration of current example
```
import control_system as mcontrol_system
import engine as mengine
import aerodynamics as maerodynamics
```
* Configure figure properties
```
plt.rcParams['figure.figsize'] = (5.0, 4.0)
plt.rcParams['image.interpolation'] = 'nearest'
plt.rcParams['image.cmap'] = 'gray'
```
* Simulation setup
```
t = 0
dt = 5e-3
tf = 10
```
* Objects definition
```
target = c4d.datapoint(x = 4000, y = 1000, z = -3000
, vx = -250, vy = 0, vz = 0)\n
```
The target is a datapoint object which means it has all the attributes of a mass in space:
Position: target.x, target.y, target.z
Velocity: target.vx, target.vy, target.vz
Acceleration: target.ax, target.ay, target.az
It also has mass: target.m (necessary for solving the equations of motion in the case of accelerating motion)
The target object in the example is initialized with initial position conditions in 3 axes and velocity conditions in the x-axis only. Acceleration is not initialized which is equivalent to zero initialization (default).
As the target in the example is non-accelerating it is not necessary to provide the mass with its instantiation.
```
missile = c4d.rigidbody(m = 85, iyy = 61, izz = 61, xcm = 1.55)
m0 = missile.m
xcm0 = missile.xcm
i0 = missile.iyy
```
The missile is a rigid-body object which means that it has all the attributes of a datapoint but in addition:
Body angles: missile.phi, missile.theta, missile.psi (Euler)
Angular rates: missile.p, missile.q, missile.r (rotation about x-body, y-body, and z-body, respectively)
Angular accelerations: missile.p_dot, missile.q_dot, missile.r_dot (about x-body, y-body, and z-body, respectively)
As a rigid-body object it has also inertia attributes (necessary for solving the equations of motion in the case of accelerating motion):
Moment of inertia: missile.ixx, missile.iyy, missile.izz (about x-body, y-body, and z-body, respectively)
Distance from nose to center of mass: missile.xcm
The missile kinematic attributes in this example have not initialized with any initial conditions which mean that all its attributes are zero. But the mass properties provided as necessary.
However, we will immediately see that the missile has initial kinematic conditions which are loaded through direct assignment and not by the class constructor (c4d.rigidbody()).
As the dynamics in this example involves a combustion of fuel of the rocket engine the missile’s mass attributes varying as function of time. To recalculate the mass during the run time it's a good advice to save these initial conditions. As iyy and izz are equal here it's enough to save just one of them.
In any way the initial position and attitude of anybody object (datapoint and rigid-body) is always saved with its instantiation
The dimensions in this example are SI (i.e. seconds, meters, kilograms), but the objects and functions in the framework in general are independent of system of units.
The example uses a line-of-sight seeker object from the c4dynamics' seekers module and controller, engine, and aerodynamics objects that developed for the purpose of this example:
```
seeker = c4d.seekers.lineofsight(dt, tau1 = 0.01, tau2 = 0.01)
ctrl = mcontrol_system.control_system(dt)
eng = mengine.engine()
aero = maerodynamics.aerodynamics()
```
* User input data:
```
#
# missile total initial velocity
##
vm = 30
#
# atmospheric properties up to 2000m
##
pressure = 101325 # pressure pascals
rho = 1.225 # density kg/m^3
vs = 340.29 # speed of sound m/s
mach = 0.8
#
# combustion properties
##
mbo = 57 # burnout mass, kg
xcmbo = 1.35 # nose to cm after burnout, m
ibo = 47 # iyy izz at burnout
```
* Preliminary calculations
The initial missile pointing direction and angular rates are calculated in the fire-control block.
For the example simulation, a simple algorithm is employed in which the missile is pointed directly at the target at the instant of launch, and missile angular rates at launch are assumed to be negligible.
The unit vector in the direction from the missile to the target is calculated by normalizing the range vector.
```
rTM = target.pos() - missile.pos()
ucl = rTM / np.linalg.norm(rTM) # center line unit vector
missile.vx, missile.vy, missile.vz = vm * ucl
missile.psi = np.arctan(ucl[1] / ucl[0])
missile.theta = np.arctan(-ucl[2] / np.sqrt(ucl[0]**2 + ucl[1]**2))
missile.phi = 0
alpha = 0
beta = 0
alpha_total = 0
h = -missile.z
```
The function missile.BI() is bounded method of the rigid-body class.
The function generates a Body from Inertial DCM (Direction Cosine Matrix) matrix in 3-2-1 order.
Using this matrix, the missile velocity vector in the inertial frame of coordinates is rotated to represent the velocity in the body frame of coordinates.
The inertial frame is determined by the frame that the initial Euler angles refer to.
In this example the Euler angles are with respect the x parallel to flat earth in the direction of the missile centerline, z positive downward, and y such that z completes a right-hand system.
The velocity vector is also produced by a bounded method of the class. The methods datapoint.pos(), datapoint.vel(), datapoint.acc() read the current state of the object and return a three entries array composed of the coordinates of position, velocity, and acceleration, respectively.
Similarly, the functions datapoint.P(), datapoint.V(), datapoint.A() return the object squared norm of the position, velocity, and acceleration, respectively.
```
u, v, w = missile.BI() @ missile.vel()
```
* Main loop
The main loop includes the following steps:
* Estimation of missile-target line-of-sight angular rate
* Production of missile's wings-deflection commands
* Calculation of missile's forces and moments
* Integration of missile's equations of motion
* Integration of target's equations of motion
* Simulation update
The simulation runs until one of the following conditions:
* The missile hits the ground
* The simulation time is over
Comments are introduced inline.
The missile.run() function and the target.run() function perform integration of the equations of motion.
The integration is performed by running Runge-Kutta of fourth order.
For a datapoint, the equations are composed of translational equations, while for a datapoint they also include the rotational equations.
Therefore, for a datapoint object, like the target in this example, a force vector is necessary for the evaluation of the derivatives. The force must be given in the inertial frame of reference. As the target in this example is not maneuvering, the force vector is [0, 0, 0].
For a rigid-body object, like the missile in this example, a force vector and moment vector are necessary for the evaluation of the derivatives. The force vector must be given in the inertial frame of reference. Therefore, the propulsion, and the aerodynamic forces are rotated to the inertial frame using the DCM321 missile.IB() function. The gravity forces are already given in the inertial frame and therefore remain intact.
Forces and moments in the inertial frame are introduced to provide an estimation of the derivatives that represent the equations of motion. The integration method, 4th order Runge-Kutta is performed manually and independent on any third-party framework (such as scipy), therefore, currently, they may be non-optimal in terms of step-size variation.
The choice of this is due to a feature which this framework desires to satisfy in which derivatives of two objects (such as missile and target) could be integrated simultaneously, with reciprocal dependent.
Hopefully, later the function will be also optimized in terms of run-time and step-size variation.
```
while t <= tf and h >= 0:
#
# atmospheric calculations
##
mach = missile.V() / vs # mach number
Q = 1 / 2 * rho * missile.V()**2 # dynamic pressure
#
# relative position
##
vTM = target.vel() - missile.vel() # missile-target relative velocity
rTM = target.pos() - missile.pos() # relative position
uR = rTM / np.linalg.norm(rTM) # unit range vector
vc = -uR * vTM # closing velocity
#
# seeker
##
wf = seeker.measure(rTM, vTM) # filtered los vector
#
# guidance and control
##
Gs = 4 * missile.V() # guidance gain
acmd = Gs * np.cross(wf, ucl) # acceleration command
ab_cmd = missile.BI() @ acmd # acc. command in body frame
afp, afy = ctrl.update(ab_cmd, Q) # acchived pithc, yaw fins deflections
d_pitch = afp - alpha # effecive pitch deflection
d_yaw = afy - beta # effective yaw deflection
#
# missile dynamics
##
#
# aerodynamic forces
##
cL, cD = aero.f_coef(alpha_total)
L = Q * aero.s * cL
D = Q * aero.s * cD
A = D * np.cos(alpha_total) - L * np.sin(alpha_total) # aero axial force
N = D * np.sin(alpha_total) + L * np.cos(alpha_total) # aero normal force
fAb = np.array([ -A
, N * (-v / np.sqrt(v**2 + w**2))
, N * (-w / np.sqrt(v**2 + w**2))])
fAe = missile.IB() @ fAb
#
# aerodynamic moments
##
cM, cN = aero.m_coef(alpha, beta, d_pitch, d_yaw
, missile.xcm, Q, missile.V(), fAb[1], fAb[2]
, missile.q, missile.r)
mA = np.array([0 # aerodynamic moemnt in roll
, Q * cM * aero.s * aero.d # aerodynamic moment in pitch
, Q * cN * aero.s * aero.d]) # aerodynamic moment in yaw
#
# propulsion
##
thrust, thref = eng.update(t, pressure)
fPb = np.array([thrust, 0, 0])
fPe = missile.IB() @ fPb
#
# gravity
##
fGe = np.array([0, 0, missile.m * c4d.params.g])
#
# total forces
##
forces = np.array([fAe[0] + fPe[0] + fGe[0]
, fAe[1] + fPe[1] + fGe[1]
, fAe[2] + fPe[2] + fGe[2]])
#
# missile motion integration
##
missile.run(dt, forces, mA)
u, v, w = missile.BI() @ np.array([missile.vx, missile.vy, missile.vz])
#
# target dynmaics
##
target.run(dt, np.array([0, 0, 0]))
#
# update
##
t += dt
missile.store(t)
target.store(t)
missile.m -= thref * dt / eng.Isp
missile.xcm = xcm0 - (xcm0 - xcmbo) * (m0 - missile.m) / (m0 - mbo)
missile.izz = missile.iyy = i0 - (i0 - ibo) * (m0 - missile.m) / (m0 - mbo)
alpha = np.arctan2(w, u)
beta = np.arctan2(-v, u)
uvm = missile.vel() / missile.V()
ucl = np.array([np.cos(missile.theta) * np.cos(missile.psi)
, np.cos(missile.theta) * np.sin(missile.psi)
, np.sin(-missile.theta)])
alpha_total = np.arccos(uvm @ ucl)
h = -missile.z
```
* Results Analysis
<!-- <p align="center">
<img src="" width="325" height="260"/>
</p>
-->
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/trajectories.png" width="325" height="260"/>
</p>
```
missile.draw('theta')
```
</p>
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/theta.png?raw=true" width="325" height="260"/>
</p>
```
target.draw('vx')
```
<p align="center">
<img src="https://github.com/C4dynamics/C4dynamics/blob/main/examples/vx.png?raw=true" width="325" height="260"/>
</p>
Raw data
{
"_id": null,
"home_page": "",
"name": "C4dynamics",
"maintainer": "",
"docs_url": null,
"requires_python": "",
"maintainer_email": "",
"keywords": "python,dynamics,physics,algorithms,computer vision,navigation",
"author": "C4dynamics",
"author_email": "zivmeri@gmail.com",
"download_url": "https://files.pythonhosted.org/packages/62/7a/e2a6a260f792bc810575dcdb2a6cffb670df32abb6633469dc1a465fdfa5/C4dynamics-0.0.15.tar.gz",
"platform": null,
"description": "\r\n<div align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/utils/C4dynamics.png\">\r\n\r\n</div>\r\n\r\n\r\n\r\n# Tsipor Dynamics\r\n\r\n## Algorithms Engineering and Development\r\n\r\n****\r\n\r\n\r\n\r\n\r\n\r\nC4Dynamics (read Tsipor (bird) Dynamics) is the open-source framework of algorithms development for objects in space and time.\r\n\r\n\r\n\r\n[](https://skillicons.dev) \r\n\r\n\r\n\r\n\r\n\r\n## Table of contents\r\n\r\n- [Motivation](https://github.com/C4dynamics/C4dynamics/tree/main/#motivation)\r\n\r\n- [Installation](https://github.com/C4dynamics/C4dynamics/tree/main/#installation)\r\n\r\n- [Quickstart](https://github.com/C4dynamics/C4dynamics/tree/main/#quickstart)\r\n\r\n- [Architecture](https://github.com/C4dynamics/C4dynamics/tree/main/#architecture)\r\n\r\n- [Contributors \u2728](https://github.com/C4dynamics/C4dynamics/tree/main/#contributors-\u2728)\r\n\r\n- [Quickstart for Contributors](https://github.com/C4dynamics/C4dynamics/tree/main/#quickstart-for-contributors)\r\n\r\n- [Getting Started 1: Objects Detection and Tracking Example](https://github.com/C4dynamics/C4dynamics/tree/main/#getting-started-1-objects-detection-and-tracking-example)\r\n\r\n- [Getting Started 2: Missile Guidance Example](https://github.com/C4dynamics/C4dynamics/tree/main/#getting-started-2-missile-guidance-example)\r\n\r\n\r\n\r\n\r\n\r\n## Motivation\r\n\r\nC4dynamics provides two basic entities for developing and analyzing algorithms of objects in space and time:\r\n\r\n* datapoint: a class defining a point in space: position, velocity, acceleration, and mass. \r\n\r\n* rigidbody: a class defining a rigid body in space, i.e. an object with length and angular position. \r\n\r\n\r\n\r\nYou can develop and analyze algorithms by operating on these objects with one of the internal systems or algorithms of C4dynamics: \r\n\r\n* ODE Solver (4th order Runge-Kutta) \r\n\r\n* Kalman Filter \r\n\r\n* Extended Kalman Filter \r\n\r\n* Luenberger Observer \r\n\r\n* Radar System \r\n\r\n* Altitude Radar \r\n\r\n* IMU Model \r\n\r\n* GPS Model \r\n\r\n* Line Of Sight Seeker \r\n\r\n \r\n\r\nOr one of the 3rd party libraries integrated with C4dynamics: \r\n\r\n* NumPy \r\n\r\n* Matplotlib \r\n\r\n* OpenCV \r\n\r\n* YOLO \r\n\r\n \r\n\r\nWhether you're a seasoned algorithm engineer or just getting started, this framework has something to offer. Its modular design allows you to easily pick and choose the components you need, and its active community of contributors is always working to improve and expand its capabilities.\r\n\r\n \r\n\r\nSo why wait? Start using C4dynamics today and take your algorithms engineering to the next level!\r\n\r\n \r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Installation \r\n\r\n* Download C4dynamics: https://github.com/C4dynamics/C4dynamics\r\n\r\n\r\n\r\n* Install the required packages:\r\n\r\n```\r\n\r\npip install -r requirements.txt\r\n\r\n```\r\n\r\n\r\n\r\n* Alternatively, run the preinstalled conda environment (see conda_installation.md):\r\n\r\n```\r\n\r\nconda env create -f c4dynamics_env.yaml\r\n\r\n```\r\n\r\n \r\n\r\n \r\n\r\n \r\n\r\n \r\n\r\n\r\n\r\n## Quickstart\r\n\r\nImport the framework:\r\n\r\n```\r\n\r\nimport C4dynamics as c4d\r\n\r\n```\r\n\r\n\r\n\r\nDefine a point in space with some initial conditions: \r\n\r\n```\r\n\r\npt = c4d.datapoint(x = 1000, vx = 100)\r\n\r\n```\r\n\r\n\r\n\r\nDefine a body in space with some initial conditions: \r\n\r\n```\r\n\r\nbody = c4d.rigidbody(theta = 15 * 3.14 / 180)\r\n\r\n```\r\n\r\n\r\n\r\nLoad an object detection module (YOLO):\r\n\r\n```\r\n\r\nyolodet = c4d.detectors.yolo(height = height, width = width)\r\n\r\n```\r\n\r\n\r\n\r\nDefine a linear Kalman Filter, perform a prediction and an update: \r\n\r\n```\r\n\r\npt.filter = c4d.filters.kalman(np.hstack((z, np.zeros(2))), P, A, H, Q, R)\r\n\r\npt.filter.predict()\r\n\r\npt.filter.correct(measure)\r\n\r\n```\r\n\r\n\r\n\r\nStore the current state of the datapoint (at time t):\r\n\r\n```\r\n\r\npt.store(t)\r\n\r\n```\r\n\r\n\r\n\r\nStore other variables added to the datapoint object:\r\n\r\n```\r\n\r\npt.storevar('kalman_state', t)\r\n\r\n```\r\n\r\n\r\n\r\nDefine errors to a general-purpose seeker with C4dynamics: \r\n\r\n```\r\n\r\nrdr = c4d.seekers.radar(sf = 0.9, bias = 0, noisestd = 1)\r\n\r\n```\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Architecture\r\n\r\nFor Architecture & Roadmap, see the Wiki page. \r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Contributors \u2728\r\n\r\n\r\n\r\n[//]: contributor-faces\r\n\r\n<a href=\"https://www.linkedin.com/in/ziv-meri/\"> <img src=\"https://github.com/C4dynamics/C4dynamics/raw/main/utils/ziv_noa2.png\" title=\"Ziv Meri\" width=\"80\" height=\"80\"></a>\r\n\r\n<a href=\"https://www.linkedin.com/in/aviva-shneor-simhon-17b733b/\"> <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/utils/aviva2.png\" title=\"Aviva Shneor Simhon\" width=\"80\" height=\"80\"></a>\r\n\r\n<a href=\"https://www.linkedin.com/in/amit-elbaz-54301382/\"> <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/utils/amit2.png\" title=\"Amit Elbaz\" width=\"80\" height=\"80\"></a>\r\n\r\n<a href=\"https://www.linkedin.com/in/avraham-ohana-computer-vision/\"> <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/utils/avraham2.png\" title=\"Avraham Ohana\" width=\"80\" height=\"80\"></a>\r\n\r\n<a href=\"https://chat.openai.com/chat\"> <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/utils/openai-featured.png\" title=\"Chat GPT\" width=\"80\" height=\"80\"></a>\r\n\r\n\r\n\r\n[//]: contributor-faces\r\n\r\n\r\n\r\nThis project follows the [all-contributors](https://github.com/all-contributors/all-contributors) specification. Contributions of any kind welcome!\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Quickstart for Contributors\r\n\r\n* See the page contributing.md\r\n\r\n* In any case, it's a good advise to start with the example dof6sim.ipynb and change the missile-target conditions to gain some experience with the framework. This example appears also down here in the README\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Getting Started 1: Objects Detection and Tracking Example\r\n\r\n### Car Detection and Tracking with YOLO and Kalman Filter using C4dynamics\r\n\r\n\r\n\r\nsee the notebook: examples\\cars_tracker.ipynb\r\n\r\n\r\n\r\nThis is an example of detecting vehicles in images using YOLO, and then tracking those detected cars using a Kalman filter. \r\n\r\n\r\n\r\nFor the demonstration, we will be using a dataset of traffic surveillance videos. The dataset contains video sequences recorded from a traffic camera, capturing various vehicles including cars.\r\n\r\n\r\n\r\n#### Let's start!\r\n\r\n\r\n\r\n* Load 3rd party modules:\r\n\r\n```\r\n\r\nimport numpy as np\r\n\r\nimport cv2\r\n\r\nfrom sklearn.neighbors import NearestNeighbors\r\n\r\nfrom matplotlib import pyplot as plt\r\n\r\nfrom matplotlib import image as mpimg\r\n\r\n```\r\n\r\n\r\n\r\n* Load the vidoes:\r\n\r\n```\r\n\r\nvideoin = os.path.join(os.getcwd(), 'examples', 'cars1.mp4')\r\n\r\nvideoout = os.path.join(os.getcwd(), 'examples', 'out', 'cars1.mp4')\r\n\r\n```\r\n\r\n\r\n\r\n* Video preprocessing:\r\n\r\n```\r\n\r\nvideo = cv2.VideoCapture(videoin)\r\n\r\ndt = 1 / video.get(cv2.CAP_PROP_FPS)\r\n\r\n_, frame1 = video.read()\r\n\r\nheight, width, _ = frame1.shape\r\n\r\nresult = cv2.VideoWriter(videoout, cv2.VideoWriter_fourcc(*'mp4v')\r\n\r\n , int(video.get(cv2.CAP_PROP_FPS)), [width, height])\r\n\r\n```\r\n\r\n\r\n\r\n```\r\n\r\nplt.imshow(mpimg.imread(os.path.join(os.getcwd(), 'examples', 'out', 'before.png')))\r\n\r\nplt.axis('off')\r\n\r\nplt.show()\r\n\r\n```\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars1.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n* Load a detector:\r\n\r\n```\r\n\r\nyolodet = c4d.detectors.yolo(height = height, width = width)\r\n\r\n```\r\n\r\n\r\n\r\n* Define Kalman parameters:\r\n\r\n```\r\n\r\n# x(t) = A(t) * x(t-1) + e(t) || e(t) ~ N(0,Q(t))\r\n\r\n# z(t) = H(t) * x(t) + r(t) || r(t) ~ N(0,R(t))\r\n\r\n\r\n\r\n# x(t) = [x1 y1 x2 y2 vx vy]\r\n\r\n\r\n\r\nA = np.array([[1, 0, 0, 0, dt, 0 ]\r\n\r\n , [0, 1, 0, 0, 0, dt]\r\n\r\n , [0, 0, 1, 0, dt, 0 ]\r\n\r\n , [0, 0, 0, 1, 0, dt]\r\n\r\n , [0, 0, 0, 0, 1, 0 ]\r\n\r\n , [0, 0, 0, 0, 0, 1 ]])\r\n\r\n\r\n\r\nH = np.array([[1, 0, 0, 0, 0, 0]\r\n\r\n , [0, 1, 0, 0, 0, 0]\r\n\r\n , [0, 0, 1, 0, 0, 0]\r\n\r\n , [0, 0, 0, 1, 0, 0]])\r\n\r\n\r\n\r\nP = np.eye(A.shape[1])\r\n\r\n\r\n\r\nQ = np.array([[25, 0, 0, 0, 0, 0]\r\n\r\n , [0, 25, 0, 0, 0, 0]\r\n\r\n , [0, 0, 25, 0, 0, 0]\r\n\r\n , [0, 0, 0, 25, 0, 0]\r\n\r\n , [0, 0, 0, 0, 49, 0]\r\n\r\n , [0, 0, 0, 0, 0, 49]])\r\n\r\n\r\n\r\nR = dt * np.eye(4)\r\n\r\n```\r\n\r\n\r\n\r\n* Define data-point object\r\n\r\nTracker = data-point + Kalman Filter\r\n\r\n\r\n\r\n```\r\n\r\nclass tracker(c4d.datapoint):\r\n\r\n # a datapoint \r\n\r\n # kalman filter \r\n\r\n # display color\r\n\r\n ## \r\n\r\n \r\n\r\n def __init__(obj, z):\r\n\r\n\r\n\r\n super().__init__() # Call the constructor of c4d.datapoint\r\n\r\n \r\n\r\n obj.filter = c4d.filters.kalman(np.hstack((z, np.zeros(2))), P, A, H, Q, R)\r\n\r\n \r\n\r\n obj.counterSame = 0\r\n\r\n obj.counterEscaped = 0\r\n\r\n obj.appear = 0\r\n\r\n\r\n\r\n obj.color = [np.random.randint(0, 255)\r\n\r\n , np.random.randint(0, 255)\r\n\r\n , np.random.randint(0, 255)]\r\n\r\n```\r\n\r\n\r\n\r\n* Define trackers manager:\r\n\r\n(a dictionary of trackers and methods to add and remove elements)\r\n\r\n\r\n\r\n```\r\n\r\n# trackers manager:\r\n\r\n# list of active trackers\r\n\r\n# add or remove tracks methods\r\n\r\nclass mTracks:\r\n\r\n def __init__(obj):\r\n\r\n obj.trackers = {}\r\n\r\n obj.removelist = []\r\n\r\n obj.neigh = NearestNeighbors(n_neighbors = 1)\r\n\r\n obj.thresh = 100 # threshold of 100 pixels to verify the measure is close enough to the object. \r\n\r\n```\r\n\r\n\r\n\r\n* Main loop:\r\n\r\n```\r\n\r\ndef run(tf = 1):\r\n\r\n \r\n\r\n mtracks = mTracks()\r\n\r\n \r\n\r\n cov = 25 * np.eye(4)\r\n\r\n t = 0\r\n\r\n \r\n\r\n while video.isOpened():\r\n\r\n \r\n\r\n ret, frame = video.read()\r\n\r\n # frame = frame.astype(np.uint8)\r\n\r\n # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) # Assuming input frames are in BGR format\r\n\r\n \r\n\r\n if not ret: \r\n\r\n break\r\n\r\n #\r\n\r\n # the update function runs the main trackers loop. \r\n\r\n ##\r\n\r\n zList = yolodet.getMeasurements(frame) # retrieves object measurements from the current frame using the object detector (YoloDetector).\r\n\r\n \r\n\r\n \r\n\r\n # updates the trackers dictionary by adding or removing tracks. \r\n\r\n # creates new trackers if there are more \r\n\r\n # measurements than trackers and removes duplicate trackers if \r\n\r\n # there are more trackers than measurements.\r\n\r\n kfNumber = len(mtracks.trackers.keys())\r\n\r\n zNumber = zList.shape[0]\r\n\r\n \r\n\r\n if kfNumber < zNumber:\r\n\r\n # more measurements than trackers\r\n\r\n mtracks.trackerMaker(zList, cov)\r\n\r\n \r\n\r\n elif kfNumber > zNumber and zNumber > 0:\r\n\r\n # more trackers than measurements\r\n\r\n mtracks.RemoveDoubles()\r\n\r\n \r\n\r\n \r\n\r\n # fits the Nearest Neighbors algorithm to the object \r\n\r\n # measurements for association purposes.\r\n\r\n if zList.shape[0] >= 2:\r\n\r\n mtracks.neigh.fit(zList)\r\n\r\n\r\n\r\n\r\n\r\n mtracks.refresh(zList, frame, t)\r\n\r\n mtracks.remove()\r\n\r\n\r\n\r\n cv2.imshow('image', frame)\r\n\r\n result.write(frame)\r\n\r\n \r\n\r\n key = cv2.waitKey(1) & 0xFF\r\n\r\n if key == ord('q') or t >= tf:\r\n\r\n break\r\n\r\n \r\n\r\n t += dt\r\n\r\n \r\n\r\n cv2.destroyAllWindows()\r\n\r\n result.release()\r\n\r\n return mtracks\r\n\r\n```\r\n\r\n\r\n\r\n* Run for two seconds:\r\n\r\n```\r\n\r\nltrk = run(tf = 2)\r\n\r\n```\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_objected_tracked.gif?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n\r\n\r\n* Results analysis:\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_id.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_trajectories.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/cars_velocity.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n## Getting Started 2: Missile Guidance Example\r\n\r\n### 6 DOF simulation of a missile employing proportional navigation guidance to pursue a target.\r\n\r\n\r\n\r\n#### Let's start: \r\n\r\n\r\n\r\n\r\n\r\n* Load third party modules \r\n\r\n\r\n\r\n```\r\n\r\nimport sys, os\r\n\r\nimport numpy as np\r\n\r\nfrom matplotlib import pyplot as plt\r\n\r\n```\r\n\r\n\r\n\r\n* Add C4dynamics to the python path and load it\r\n\r\n\r\n\r\n```\r\n\r\nsys.path.append(os.path.join(os.getcwd(), '..', '..'))\r\n\r\nimport C4dynamics as c4d\r\n\r\n```\r\n\r\n\r\n\r\n* Load other modules that developed for the demonstration of current example \r\n\r\n\r\n\r\n```\r\n\r\nimport control_system as mcontrol_system \r\n\r\nimport engine as mengine \r\n\r\nimport aerodynamics as maerodynamics\r\n\r\n```\r\n\r\n\r\n\r\n* Configure figure properties \r\n\r\n\r\n\r\n\r\n\r\n```\r\n\r\nplt.rcParams['figure.figsize'] = (5.0, 4.0) \r\n\r\nplt.rcParams['image.interpolation'] = 'nearest'\r\n\r\nplt.rcParams['image.cmap'] = 'gray'\r\n\r\n```\r\n\r\n\r\n\r\n* Simulation setup\r\n\r\n\r\n\r\n```\r\n\r\nt = 0\r\n\r\ndt = 5e-3\r\n\r\ntf = 10 \r\n\r\n```\r\n\r\n\r\n\r\n* Objects definition \r\n\r\n\r\n\r\n\r\n\r\n```\r\n\r\ntarget = c4d.datapoint(x = 4000, y = 1000, z = -3000\r\n\r\n , vx = -250, vy = 0, vz = 0)\\n\r\n\r\n\r\n\r\n```\r\n\r\n\r\n\r\nThe target is a datapoint object which means it has all the attributes of a mass in space:\r\n\r\n\r\n\r\nPosition: target.x, target.y, target.z\r\n\r\n\r\n\r\nVelocity: target.vx, target.vy, target.vz\r\n\r\n\r\n\r\nAcceleration: target.ax, target.ay, target.az\r\n\r\n\r\n\r\nIt also has mass: target.m (necessary for solving the equations of motion in the case of accelerating motion)\r\n\r\n\r\n\r\n\r\n\r\nThe target object in the example is initialized with initial position conditions in 3 axes and velocity conditions in the x-axis only. Acceleration is not initialized which is equivalent to zero initialization (default).\r\n\r\nAs the target in the example is non-accelerating it is not necessary to provide the mass with its instantiation.\r\n\r\n\r\n\r\n```\r\n\r\nmissile = c4d.rigidbody(m = 85, iyy = 61, izz = 61, xcm = 1.55)\r\n\r\n\r\n\r\nm0 = missile.m\r\n\r\nxcm0 = missile.xcm\r\n\r\ni0 = missile.iyy\r\n\r\n```\r\n\r\n\r\n\r\n\r\n\r\nThe missile is a rigid-body object which means that it has all the attributes of a datapoint but in addition:\r\n\r\n\r\n\r\nBody angles: missile.phi, missile.theta, missile.psi (Euler)\r\n\r\n\r\n\r\nAngular rates: missile.p, missile.q, missile.r (rotation about x-body, y-body, and z-body, respectively)\r\n\r\n\r\n\r\nAngular accelerations: missile.p_dot, missile.q_dot, missile.r_dot (about x-body, y-body, and z-body, respectively)\r\n\r\n\r\n\r\n\r\n\r\nAs a rigid-body object it has also inertia attributes (necessary for solving the equations of motion in the case of accelerating motion):\r\n\r\n\r\n\r\nMoment of inertia: missile.ixx, missile.iyy, missile.izz (about x-body, y-body, and z-body, respectively)\r\n\r\n\r\n\r\nDistance from nose to center of mass: missile.xcm\r\n\r\n\r\n\r\n\r\n\r\nThe missile kinematic attributes in this example have not initialized with any initial conditions which mean that all its attributes are zero. But the mass properties provided as necessary. \r\n\r\n\r\n\r\nHowever, we will immediately see that the missile has initial kinematic conditions which are loaded through direct assignment and not by the class constructor (c4d.rigidbody()).\r\n\r\n\r\n\r\n\r\n\r\nAs the dynamics in this example involves a combustion of fuel of the rocket engine the missile\u2019s mass attributes varying as function of time. To recalculate the mass during the run time it's a good advice to save these initial conditions. As iyy and izz are equal here it's enough to save just one of them.\r\n\r\n\r\n\r\nIn any way the initial position and attitude of anybody object (datapoint and rigid-body) is always saved with its instantiation \r\n\r\n\r\n\r\nThe dimensions in this example are SI (i.e. seconds, meters, kilograms), but the objects and functions in the framework in general are independent of system of units. \r\n\r\n\r\n\r\nThe example uses a line-of-sight seeker object from the c4dynamics' seekers module and controller, engine, and aerodynamics objects that developed for the purpose of this example:\r\n\r\n\r\n\r\n```\r\n\r\nseeker = c4d.seekers.lineofsight(dt, tau1 = 0.01, tau2 = 0.01)\r\n\r\nctrl = mcontrol_system.control_system(dt)\r\n\r\neng = mengine.engine()\r\n\r\naero = maerodynamics.aerodynamics()\r\n\r\n```\r\n\r\n\r\n\r\n\r\n\r\n* User input data:\r\n\r\n\r\n\r\n```\r\n\r\n# \r\n\r\n# missile total initial velocity \r\n\r\n##\r\n\r\nvm = 30 \r\n\r\n\r\n\r\n# \r\n\r\n# atmospheric properties up to 2000m\r\n\r\n##\r\n\r\npressure = 101325 # pressure pascals\r\n\r\nrho = 1.225 # density kg/m^3\r\n\r\nvs = 340.29 # speed of sound m/s\r\n\r\nmach = 0.8\r\n\r\n\r\n\r\n# \r\n\r\n# combustion properties \r\n\r\n## \r\n\r\nmbo = 57 # burnout mass, kg \r\n\r\nxcmbo = 1.35 # nose to cm after burnout, m\r\n\r\nibo = 47 # iyy izz at burnout \r\n\r\n```\r\n\r\n\r\n\r\n\r\n\r\n* Preliminary calculations \r\n\r\n\r\n\r\n\r\n\r\nThe initial missile pointing direction and angular rates are calculated in the fire-control block. \r\n\r\n\r\n\r\nFor the example simulation, a simple algorithm is employed in which the missile is pointed directly at the target at the instant of launch, and missile angular rates at launch are assumed to be negligible.\r\n\r\n\r\n\r\n\r\n\r\nThe unit vector in the direction from the missile to the target is calculated by normalizing the range vector.\r\n\r\n\r\n\r\n \r\n\r\n```\r\n\r\nrTM = target.pos() - missile.pos()\r\n\r\nucl = rTM / np.linalg.norm(rTM) # center line unit vector \r\n\r\nmissile.vx, missile.vy, missile.vz = vm * ucl \r\n\r\nmissile.psi = np.arctan(ucl[1] / ucl[0])\r\n\r\nmissile.theta = np.arctan(-ucl[2] / np.sqrt(ucl[0]**2 + ucl[1]**2))\r\n\r\nmissile.phi = 0\r\n\r\nalpha = 0\r\n\r\nbeta = 0\r\n\r\nalpha_total = 0\r\n\r\nh = -missile.z\r\n\r\n```\r\n\r\n\r\n\r\nThe function missile.BI() is bounded method of the rigid-body class.\r\n\r\n\r\n\r\nThe function generates a Body from Inertial DCM (Direction Cosine Matrix) matrix in 3-2-1 order.\r\n\r\n\r\n\r\nUsing this matrix, the missile velocity vector in the inertial frame of coordinates is rotated to represent the velocity in the body frame of coordinates.\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\nThe inertial frame is determined by the frame that the initial Euler angles refer to.\r\n\r\n\r\n\r\nIn this example the Euler angles are with respect the x parallel to flat earth in the direction of the missile centerline, z positive downward, and y such that z completes a right-hand system.\r\n\r\n\r\n\r\nThe velocity vector is also produced by a bounded method of the class. The methods datapoint.pos(), datapoint.vel(), datapoint.acc() read the current state of the object and return a three entries array composed of the coordinates of position, velocity, and acceleration, respectively. \r\n\r\n\r\n\r\nSimilarly, the functions datapoint.P(), datapoint.V(), datapoint.A() return the object squared norm of the position, velocity, and acceleration, respectively.\r\n\r\n\r\n\r\n```\r\n\r\nu, v, w = missile.BI() @ missile.vel()\r\n\r\n```\r\n\r\n\r\n\r\n* Main loop\r\n\r\n\r\n\r\n\r\n\r\nThe main loop includes the following steps: \r\n\r\n\r\n\r\n* Estimation of missile-target line-of-sight angular rate \r\n\r\n\t\r\n\r\n* Production of missile's wings-deflection commands \r\n\r\n\t\r\n\r\n* Calculation of missile's forces and moments \r\n\r\n\t\r\n\r\n* Integration of missile's equations of motion \r\n\r\n\t\r\n\r\n* Integration of target's equations of motion \r\n\r\n\t\r\n\r\n* Simulation update \r\n\r\n\r\n\r\n\r\n\r\nThe simulation runs until one of the following conditions:\r\n\r\n\r\n\r\n* The missile hits the ground\r\n\r\n\t\r\n\r\n* The simulation time is over \r\n\r\n\t\r\n\r\n\r\n\r\n\r\n\r\nComments are introduced inline.\r\n\r\n\r\n\r\nThe missile.run() function and the target.run() function perform integration of the equations of motion. \r\n\r\n\r\n\r\nThe integration is performed by running Runge-Kutta of fourth order. \r\n\r\n\r\n\r\nFor a datapoint, the equations are composed of translational equations, while for a datapoint they also include the rotational equations. \r\n\r\n\r\n\r\n\r\n\r\nTherefore, for a datapoint object, like the target in this example, a force vector is necessary for the evaluation of the derivatives. The force must be given in the inertial frame of reference. As the target in this example is not maneuvering, the force vector is [0, 0, 0].\r\n\r\n\r\n\r\n\r\n\r\nFor a rigid-body object, like the missile in this example, a force vector and moment vector are necessary for the evaluation of the derivatives. The force vector must be given in the inertial frame of reference. Therefore, the propulsion, and the aerodynamic forces are rotated to the inertial frame using the DCM321 missile.IB() function. The gravity forces are already given in the inertial frame and therefore remain intact. \r\n\r\n\r\n\r\n\r\n\r\nForces and moments in the inertial frame are introduced to provide an estimation of the derivatives that represent the equations of motion. The integration method, 4th order Runge-Kutta is performed manually and independent on any third-party framework (such as scipy), therefore, currently, they may be non-optimal in terms of step-size variation. \r\n\r\n\r\n\r\nThe choice of this is due to a feature which this framework desires to satisfy in which derivatives of two objects (such as missile and target) could be integrated simultaneously, with reciprocal dependent. \r\n\r\n\r\n\r\nHopefully, later the function will be also optimized in terms of run-time and step-size variation.\r\n\r\n\r\n\r\n```\r\n\r\nwhile t <= tf and h >= 0:\r\n\r\n\r\n\r\n #\r\n\r\n # atmospheric calculations \r\n\r\n ##\r\n\r\n mach = missile.V() / vs # mach number \r\n\r\n Q = 1 / 2 * rho * missile.V()**2 # dynamic pressure \r\n\r\n \r\n\r\n # \r\n\r\n # relative position\r\n\r\n ##\r\n\r\n vTM = target.vel() - missile.vel() # missile-target relative velocity \r\n\r\n rTM = target.pos() - missile.pos() # relative position \r\n\r\n uR = rTM / np.linalg.norm(rTM) # unit range vector \r\n\r\n vc = -uR * vTM # closing velocity \r\n\r\n\r\n\r\n # \r\n\r\n # seeker \r\n\r\n ## \r\n\r\n wf = seeker.measure(rTM, vTM) # filtered los vector \r\n\r\n \r\n\r\n # \r\n\r\n # guidance and control \r\n\r\n ##\r\n\r\n Gs = 4 * missile.V() # guidance gain\r\n\r\n acmd = Gs * np.cross(wf, ucl) # acceleration command\r\n\r\n ab_cmd = missile.BI() @ acmd # acc. command in body frame \r\n\r\n afp, afy = ctrl.update(ab_cmd, Q) # acchived pithc, yaw fins deflections \r\n\r\n d_pitch = afp - alpha # effecive pitch deflection\r\n\r\n d_yaw = afy - beta # effective yaw deflection \r\n\r\n\r\n\r\n\r\n\r\n # \r\n\r\n # missile dynamics \r\n\r\n ##\r\n\r\n \r\n\r\n #\r\n\r\n # aerodynamic forces \r\n\r\n ## \r\n\r\n cL, cD = aero.f_coef(alpha_total)\r\n\r\n L = Q * aero.s * cL\r\n\r\n D = Q * aero.s * cD\r\n\r\n \r\n\r\n A = D * np.cos(alpha_total) - L * np.sin(alpha_total) # aero axial force \r\n\r\n N = D * np.sin(alpha_total) + L * np.cos(alpha_total) # aero normal force \r\n\r\n \r\n\r\n fAb = np.array([ -A\r\n\r\n , N * (-v / np.sqrt(v**2 + w**2))\r\n\r\n , N * (-w / np.sqrt(v**2 + w**2))])\r\n\r\n fAe = missile.IB() @ fAb\r\n\r\n \r\n\r\n # \r\n\r\n # aerodynamic moments \r\n\r\n ##\r\n\r\n cM, cN = aero.m_coef(alpha, beta, d_pitch, d_yaw \r\n\r\n , missile.xcm, Q, missile.V(), fAb[1], fAb[2]\r\n\r\n , missile.q, missile.r)\r\n\r\n \r\n\r\n mA = np.array([0 # aerodynamic moemnt in roll\r\n\r\n , Q * cM * aero.s * aero.d # aerodynamic moment in pitch\r\n\r\n , Q * cN * aero.s * aero.d]) # aerodynamic moment in yaw \r\n\r\n\r\n\r\n # \r\n\r\n # propulsion \r\n\r\n ##\r\n\r\n thrust, thref = eng.update(t, pressure)\r\n\r\n fPb = np.array([thrust, 0, 0]) \r\n\r\n fPe = missile.IB() @ fPb\r\n\r\n\r\n\r\n # \r\n\r\n # gravity\r\n\r\n ## \r\n\r\n fGe = np.array([0, 0, missile.m * c4d.params.g])\r\n\r\n\r\n\r\n # \r\n\r\n # total forces\r\n\r\n ## \r\n\r\n forces = np.array([fAe[0] + fPe[0] + fGe[0]\r\n\r\n , fAe[1] + fPe[1] + fGe[1]\r\n\r\n , fAe[2] + fPe[2] + fGe[2]])\r\n\r\n \r\n\r\n # \r\n\r\n # missile motion integration\r\n\r\n ##\r\n\r\n missile.run(dt, forces, mA)\r\n\r\n u, v, w = missile.BI() @ np.array([missile.vx, missile.vy, missile.vz])\r\n\r\n \r\n\r\n \r\n\r\n # \r\n\r\n # target dynmaics \r\n\r\n ##\r\n\r\n target.run(dt, np.array([0, 0, 0]))\r\n\r\n\r\n\r\n\r\n\r\n # \r\n\r\n # update \r\n\r\n ##\r\n\r\n t += dt\r\n\r\n missile.store(t)\r\n\r\n target.store(t)\r\n\r\n\r\n\r\n \r\n\r\n missile.m -= thref * dt / eng.Isp \r\n\r\n missile.xcm = xcm0 - (xcm0 - xcmbo) * (m0 - missile.m) / (m0 - mbo)\r\n\r\n missile.izz = missile.iyy = i0 - (i0 - ibo) * (m0 - missile.m) / (m0 - mbo)\r\n\r\n\r\n\r\n alpha = np.arctan2(w, u)\r\n\r\n beta = np.arctan2(-v, u)\r\n\r\n \r\n\r\n uvm = missile.vel() / missile.V()\r\n\r\n ucl = np.array([np.cos(missile.theta) * np.cos(missile.psi)\r\n\r\n , np.cos(missile.theta) * np.sin(missile.psi)\r\n\r\n , np.sin(-missile.theta)])\r\n\r\n alpha_total = np.arccos(uvm @ ucl)\r\n\r\n \r\n\r\n h = -missile.z \r\n\r\n```\r\n\r\n\r\n\r\n* Results Analysis \r\n\r\n\r\n\r\n<!-- <p align=\"center\">\r\n\r\n <img src=\"\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n -->\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/trajectories.png\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n\r\n\r\n```\r\n\r\nmissile.draw('theta')\r\n\r\n```\r\n\r\n</p>\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/theta.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n```\r\n\r\ntarget.draw('vx')\r\n\r\n```\r\n\r\n<p align=\"center\">\r\n\r\n <img src=\"https://github.com/C4dynamics/C4dynamics/blob/main/examples/vx.png?raw=true\" width=\"325\" height=\"260\"/>\r\n\r\n</p>\r\n\r\n\r\n\r\n\r\n\r\n",
"bugtrack_url": null,
"license": "",
"summary": "The framework for algorithms engineering with Python.",
"version": "0.0.15",
"project_urls": null,
"split_keywords": [
"python",
"dynamics",
"physics",
"algorithms",
"computer vision",
"navigation"
],
"urls": [
{
"comment_text": "",
"digests": {
"blake2b_256": "5190a6c3b22681afea8179b5c7e67ac5dbfc4b411e15f77868cd3fad4d815516",
"md5": "d181f13d41395613afa3dc440182d265",
"sha256": "4070f83ad1d75b7adc184bb2ea2fb50575e73c027886596862a3e09511979431"
},
"downloads": -1,
"filename": "C4dynamics-0.0.15-py3-none-any.whl",
"has_sig": false,
"md5_digest": "d181f13d41395613afa3dc440182d265",
"packagetype": "bdist_wheel",
"python_version": "py3",
"requires_python": null,
"size": 34479,
"upload_time": "2023-11-08T17:08:58",
"upload_time_iso_8601": "2023-11-08T17:08:58.224186Z",
"url": "https://files.pythonhosted.org/packages/51/90/a6c3b22681afea8179b5c7e67ac5dbfc4b411e15f77868cd3fad4d815516/C4dynamics-0.0.15-py3-none-any.whl",
"yanked": false,
"yanked_reason": null
},
{
"comment_text": "",
"digests": {
"blake2b_256": "627ae2a6a260f792bc810575dcdb2a6cffb670df32abb6633469dc1a465fdfa5",
"md5": "db6814a6510e2a24d78bc9410dd68dc2",
"sha256": "0e895c4c22d4f1357f0afdeb830e64fea854a6b1a86c9917055e2f63e8ff75d9"
},
"downloads": -1,
"filename": "C4dynamics-0.0.15.tar.gz",
"has_sig": false,
"md5_digest": "db6814a6510e2a24d78bc9410dd68dc2",
"packagetype": "sdist",
"python_version": "source",
"requires_python": null,
"size": 34387,
"upload_time": "2023-11-08T17:09:00",
"upload_time_iso_8601": "2023-11-08T17:09:00.020603Z",
"url": "https://files.pythonhosted.org/packages/62/7a/e2a6a260f792bc810575dcdb2a6cffb670df32abb6633469dc1a465fdfa5/C4dynamics-0.0.15.tar.gz",
"yanked": false,
"yanked_reason": null
}
],
"upload_time": "2023-11-08 17:09:00",
"github": false,
"gitlab": false,
"bitbucket": false,
"codeberg": false,
"lcname": "c4dynamics"
}
JSON