Skip to content

Commit

Permalink
Complete example, GPS and collision risk
Browse files Browse the repository at this point in the history
  • Loading branch information
@n-akram
n-akram committed Oct 2, 2023
1 parent 506a74a commit 610fa84
Show file tree
Hide file tree
Showing 2 changed files with 215 additions and 70 deletions.
130 changes: 97 additions & 33 deletions SafeDrones/core/SafeDrones.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,13 +33,15 @@ def __init__(self):
self.danger_threshold = None
self.collision_threshold = None
self.GPS_Lambda = None
self.SatStatus = None

self.Set_Variables()

def Set_Variables(self, MotorStatus=[1,1,1,1,1,1], Motors_Configuration='PNPNPN',\
Motors_Lambda = 0.001, Batt_Lambda = 0.001, alpha = 0.008, beta = 0.007, \
Battery_degradation_rate = 0.0064, BatteryLevel=80, MTTFref=400, \
Tr=30, Ta=50, u=1,b=1, time=100, danger_threshold=2, collision_threshold=0, GPS_Lambda=0.001):
Tr=30, Ta=50, u=1,b=1, time=100, danger_threshold=2, collision_threshold=0, \
GPS_Lambda=0.001, SatStatus = 0):
#%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# Version : 1.0.0 %
# Description : set variables used in the program %
Expand Down Expand Up @@ -72,6 +74,7 @@ def Set_Variables(self, MotorStatus=[1,1,1,1,1,1], Motors_Configuration='PNPNPN'
self.danger_threshold = danger_threshold
self.collision_threshold = collision_threshold
self.GPS_Lambda = GPS_Lambda
self.SatStatus = SatStatus


def Motor_Failure_Risk_Calc(self, MotorStatus=None, Motors_Configuration=None, Motors_Lambda=None, time=None):
Expand Down Expand Up @@ -587,45 +590,106 @@ def Chip_MTTF_Model(self,MTTFref=None, Tr=None, Ta = None, u=None,b = None, time

return P_Fail.evalf(subs={t: time}), MTTFchip.evalf(subs={t: time})

def GPS_Failure_Risk_Calc(self):
L = self.GPS_Lambda
t = self.time

M = np.array([[-14*L, 0, 0, 0, 0, 0, 0, 0, 0],
[ 14*L, -13*L, 0, 0, 0, 0, 0, 0, 0],
[ 0, 13*L, -12*L, 0, 0, 0, 0, 0, 0],
[ 0, 0, 12*L, -11*L, 0, 0, 0, 0, 0],
[ 0, 0, 0, 11*L, -10*L, 0, 0, 0, 0],
[ 0, 0, 0, 0, 10*L, -9*L, 0, 0, 0],
[ 0, 0, 0, 0, 0, 9*L, -8*L, 0, 0],
[ 0, 0, 0, 0, 0, 0, 8*L, -7*L, 0],
[ 0, 0, 0, 0, 0, 0, 0, 7*L, 0]])

P = np.dot(sym.exp(M*t), self.SatStatus)
def GPS_Failure_Risk_Calc(self, SatStatus=None, time=None, Lambda = None, ):

P_Fail = P[-1]

N = np.array([[-14*L, 0, 0, 0, 0, 0, 0, 0],
[ 14*L, -13*L, 0, 0, 0, 0, 0, 0],
[ 0, 13*L, -12*L, 0, 0, 0, 0, 0],
[ 0, 0, 12*L, -11*L, 0, 0, 0, 0],
[ 0, 0, 0, 11*L, -10*L, 0, 0, 0],
[ 0, 0, 0, 0, 10*L, -9*L, 0, 0],
[ 0, 0, 0, 0, 0, 9*L, -8*L, 0],
[ 0, 0, 0, 0, 0, 0, 8*L, -7*L]])

Time_Symbolic = np.sum(np.sum(-1.*np.linalg.inv(N), axis=1)*(self.SatStatus[:8]))

MTTF = Time_Symbolic

return P_Fail, MTTF
#%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# Program Name : Markov-based Drone's Reliability and MTTF Estimator %
# Author : Koorosh Aslansefat %
# Version : 1.0.0 %
# Description : A Markov Process-Based Approach for Reliability %
# Evaluation of the GPS System for Drones %
#%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

'''
SatStatus: is an integer between 0 and 11. 0 means no satellite is available and 14 means fully satellite coverage.
Lambda: Failure Rate of the GPS System.
time: Time of the mission
'''

import numpy as np # linear algebra
import pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)
import sympy as sym # Symbolic Calculation

t = sym.Symbol('t')

if SatStatus == None:
SatStatus = self.SatStatus
if Lambda == None:
Lambda = self.GPS_Lambda
L = Lambda

def calculate_collision_risk(self, uav_1_trajectory, uav_2_trajectory): # self.danger_threshold, self.collision_threshold
if SatStatus == 14:
P0_GPS = sym.Matrix([[1],[0],[0],[0],[0],[0],[0],[0],[0]])
Sflag = 7
elif SatStatus == 13:
P0_GPS = sym.Matrix([[0],[1],[0],[0],[0],[0],[0],[0],[0]])
Sflag = 6
elif SatStatus == 12:
P0_GPS = sym.Matrix([[0],[0],[1],[0],[0],[0],[0],[0],[0]])
Sflag = 5
elif SatStatus == 11:
P0_GPS = sym.Matrix([[0],[0],[0],[1],[0],[0],[0],[0],[0]])
Sflag = 4
elif SatStatus == 10:
P0_GPS = sym.Matrix([[0],[0],[0],[0],[1],[0],[0],[0],[0]])
Sflag = 3
elif SatStatus == 9:
P0_GPS = sym.Matrix([[0],[0],[0],[0],[0],[1],[0],[0],[0]])
Sflag = 2
elif SatStatus == 8:
P0_GPS = sym.Matrix([[0],[0],[0],[0],[0],[0],[1],[0],[0]])
Sflag = 1
elif SatStatus == 7:
P0_GPS = sym.Matrix([[0],[0],[0],[0],[0],[0],[0],[1],[0]])
Sflag = 0
else:
P_Fail = 1
MTTF = 0
return P_Fail, MTTF

M_GPS = sym.Matrix([[-14*L, 0, 0, 0, 0, 0, 0, 0, 0],
[ 14*L, -13*L, 0, 0, 0, 0, 0, 0, 0],
[ 0, 13*L, -12*L, 0, 0, 0, 0, 0, 0],
[ 0, 0, 12*L, -11*L, 0, 0, 0, 0, 0],
[ 0, 0, 0, 11*L, -10*L, 0, 0, 0, 0],
[ 0, 0, 0, 0, 10*L, -9*L, 0, 0, 0],
[ 0, 0, 0, 0, 0, 9*L, -8*L, 0, 0],
[ 0, 0, 0, 0, 0, 0, 8*L, -7*L, 0],
[ 0, 0, 0, 0, 0, 0, 0, 7*L, 0]])

P_GPS = sym.exp(M_GPS*t)*P0_GPS

P_GPS_Fail = P_GPS[-1]

N_GPS = sym.Matrix([[-14*L, 0, 0, 0, 0, 0, 0, 0],
[ 14*L, -13*L, 0, 0, 0, 0, 0, 0],
[ 0, 13*L, -12*L, 0, 0, 0, 0, 0],
[ 0, 0, 12*L, -11*L, 0, 0, 0, 0],
[ 0, 0, 0, 11*L, -10*L, 0, 0, 0],
[ 0, 0, 0, 0, 10*L, -9*L, 0, 0],
[ 0, 0, 0, 0, 0, 9*L, -8*L, 0],
[ 0, 0, 0, 0, 0, 0, 8*L, -7*L]])

tt = -1*N_GPS.inv()

MTTF_GPS = sum(tt[Sflag,:])

return P_GPS_Fail.evalf(subs={t: time}), MTTF_GPS.evalf(subs={t: time})

def calculate_collision_risk(self, uav_1_trajectory, uav_2_trajectory, danger_threshold=-1, collision_threshold=-1): # self.danger_threshold, self.collision_threshold
# Check that both trajectories have the same length
assert len(uav_1_trajectory) == len(uav_2_trajectory), "Trajectories must have the same length"

danger_zone_count = 0
collision_zone_count = 0

if danger_threshold != -1:
self.danger_threshold = danger_threshold
if collision_threshold != -1:
self.collision_threshold = collision_threshold

for point_1, point_2 in zip(uav_1_trajectory, uav_2_trajectory):
distance = np.linalg.norm(np.array(point_1) - np.array(point_2))
Expand Down
155 changes: 118 additions & 37 deletions example/SafeDrone-demo-2.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,9 @@
"from IPython.display import Markdown as md\n",
"\n",
"from sys import platform\n",
"import pandas as pd\n",
"import numpy as np\n",
"import random\n",
"\n",
"if platform == \"linux\" or platform == \"linux2\":\n",
" currentdir = os.path.dirname(os.path.realpath(inspect.getfile(inspect.currentframe())))\n",
Expand All @@ -48,7 +51,15 @@
"metadata": {},
"outputs": [],
"source": [
"import numpy as np"
"eval = SafeDrones()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Initialize the variables . . . .\n",
"Use $5$ as collision threshold, and $10$ as danger threshold. GPS lambda can be used the default ($0.001$)."
]
},
{
Expand All @@ -57,98 +68,168 @@
"metadata": {},
"outputs": [],
"source": [
"eval = SafeDrones()"
"eval.Set_Variables(collision_threshold=5, danger_threshold=10, GPS_Lambda=0.001)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Initialize the variables . . . .\n",
"Use $5$ as collision threshold, and $10$ as danger threshold. GPS lambda can be used the default ($0.001$)."
"Sample trajectories . . . . "
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"0.5\n",
"0.5\n"
]
}
],
"source": [
"eval.Set_Variables(collision_threshold=5, danger_threshold=10, GPS_Lambda=0.001)"
"uav_1_trajectory = [(x, 0, 0) for x in range(10)]\n",
"uav_2_trajectory = [(x, 0, x) for x in range(10)]\n",
"\n",
"danger_zone_risk, collision_zone_risk = eval.calculate_collision_risk(uav_1_trajectory, uav_2_trajectory)\n",
"\n",
"print(danger_zone_risk)\n",
"print(collision_zone_risk)"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"0.3\n",
"0.2\n"
]
}
],
"source": [
"danger_threshold = 5\n",
"collision_threshold = 2\n",
"\n",
"danger_zone_risk, collision_zone_risk = eval.calculate_collision_risk(uav_1_trajectory, uav_2_trajectory, danger_threshold, collision_threshold)\n",
"\n",
"print(danger_zone_risk)\n",
"print(collision_zone_risk)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Sample trajectories . . . . "
"To check the risk of GPS failure, let us first set the satellite status. The satellite status is a number between 0 to 18. Let us randomly assign the the status . . . ."
]
},
{
"cell_type": "code",
"execution_count": 5,
"execution_count": 6,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"The satellite status is set as : 8\n"
]
}
],
"source": [
"traj_1 = np.array([1, 2, 3, 4, 5, 6])\n",
"traj_2 = np.array([1, 2, 3, 4, 5, 6])"
"sat_status = random.randint(0, 18)\n",
"eval.Set_Variables(SatStatus = sat_status)\n",
"print(\"The satellite status is set as : \", sat_status)"
]
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 7,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(0.0, 1.0)"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
"name": "stdout",
"output_type": "stream",
"text": [
"0.172620318489205\n",
"153.846153846154\n"
]
}
],
"source": [
"risk = eval.calculate_collision_risk(traj_1, traj_2)\n",
"risk"
"GPS_P_Fail, GPS_MTTF = eval.GPS_Failure_Risk_Calc(time=100)\n",
"print(GPS_P_Fail)\n",
"print(GPS_MTTF)"
]
},
{
"cell_type": "markdown",
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"1.19038762535297e-5\n",
"1142.85714285714\n"
]
}
],
"source": [
"Check the risk of GPS failure . . . . "
"GPS_P_Fail, GPS_MTTF = eval.GPS_Failure_Risk_Calc(SatStatus=14, time=100, Lambda = 0.001)\n",
"print(GPS_P_Fail)\n",
"print(GPS_MTTF)"
]
},
{
"cell_type": "markdown",
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"0.0107555729878382\n",
"363.636363636364\n"
]
}
],
"source": [
"## INCOMPLETE . . . ."
"GPS_P_Fail, GPS_MTTF = eval.GPS_Failure_Risk_Calc(SatStatus=10, time=100, Lambda = 0.001)\n",
"print(GPS_P_Fail)\n",
"print(GPS_MTTF)"
]
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 10,
"metadata": {},
"outputs": [
{
"ename": "AttributeError",
"evalue": "'SafeDrones' object has no attribute 'SatStatus'",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
"\u001b[1;32mc:\\AKRAM-Local\\github\\SafeDrones\\example\\SafeDrone-demo-2.ipynb Cell 12\u001b[0m line \u001b[0;36m<cell line: 1>\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> <a href='vscode-notebook-cell:/c%3A/AKRAM-Local/github/SafeDrones/example/SafeDrone-demo-2.ipynb#X15sZmlsZQ%3D%3D?line=0'>1</a>\u001b[0m gps_risk \u001b[39m=\u001b[39m \u001b[39meval\u001b[39;49m\u001b[39m.\u001b[39;49mGPS_Failure_Risk_Calc()\n",
"File \u001b[1;32mc:\\AKRAM-Local\\github\\SafeDrones\\SafeDrones\\core\\SafeDrones.py:604\u001b[0m, in \u001b[0;36mSafeDrones.GPS_Failure_Risk_Calc\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m 592\u001b[0m t \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mtime\n\u001b[0;32m 594\u001b[0m M \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray([[\u001b[39m-\u001b[39m\u001b[39m14\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[0;32m 595\u001b[0m [ \u001b[39m14\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m13\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[0;32m 596\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m13\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m12\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[1;32m (...)\u001b[0m\n\u001b[0;32m 601\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m8\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m7\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m],\n\u001b[0;32m 602\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m7\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m]])\n\u001b[1;32m--> 604\u001b[0m P \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39mdot(sym\u001b[39m.\u001b[39mexp(M\u001b[39m*\u001b[39mt), \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mSatStatus)\n\u001b[0;32m 606\u001b[0m P_Fail \u001b[39m=\u001b[39m P[\u001b[39m-\u001b[39m\u001b[39m1\u001b[39m]\n\u001b[0;32m 608\u001b[0m N \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray([[\u001b[39m-\u001b[39m\u001b[39m14\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[0;32m 609\u001b[0m [ \u001b[39m14\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m13\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[0;32m 610\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m13\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m12\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m],\n\u001b[1;32m (...)\u001b[0m\n\u001b[0;32m 614\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m9\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m8\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m0\u001b[39m],\n\u001b[0;32m 615\u001b[0m [ \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m0\u001b[39m, \u001b[39m8\u001b[39m\u001b[39m*\u001b[39mL, \u001b[39m-\u001b[39m\u001b[39m7\u001b[39m\u001b[39m*\u001b[39mL]])\n",
"\u001b[1;31mAttributeError\u001b[0m: 'SafeDrones' object has no attribute 'SatStatus'"
"name": "stdout",
"output_type": "stream",
"text": [
"1\n",
"0\n"
]
}
],
"source": [
"gps_risk = eval.GPS_Failure_Risk_Calc()"
"GPS_P_Fail, GPS_MTTF = eval.GPS_Failure_Risk_Calc(SatStatus=0, time=100, Lambda = 0.001)\n",
"print(GPS_P_Fail)\n",
"print(GPS_MTTF)"
]
}
],
Expand Down

0 comments on commit 610fa84

Please sign in to comment.