1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
|
-- | This example shows an implementation of the Well-Clear Violation
-- algorithm, it follows the implementation described in 'Analysis of
-- Well-Clear Bounday Models for the Integration of UAS in the NAS',
-- https://ntrs.nasa.gov/citations/20140010078.
{-# LANGUAGE DataKinds #-}
{-# LANGUAGE RebindableSyntax #-}
module Main where
import Language.Copilot
import qualified Copilot.Theorem.What4 as CT
import qualified Prelude as P
import Data.Foldable (forM_)
import qualified Control.Monad as Monad
-- | `dthr` is the horizontal distance threshold.
dthr :: Stream Double
dthr = extern "dthr" Nothing
-- | `tthr` is the horizontal time threshold.
tthr :: Stream Double
tthr = extern "tthr" Nothing
-- | `zthr` is the vertical distance / altitude threshold.
zthr :: Stream Double
zthr = extern "zthr" Nothing
-- | `tcoathr` is the vertical time threshold.
tcoathr :: Stream Double
tcoathr = extern "tcoathr" Nothing
type Vect2 = (Stream Double, Stream Double)
-- External streams for relative position and velocity.
-- | The relative x velocity between ownship and the intruder.
vx :: Stream Double
vx = extern "relative_velocity_x" Nothing
-- | The relative y velocity between ownship and the intruder.
vy :: Stream Double
vy = extern "relative_velocity_y" Nothing
-- | The relative z velocity between ownship and the intruder.
vz :: Stream Double
vz = extern "relative_velocity_z" Nothing
-- | The relative velocity as a 2D vector.
v :: (Stream Double, Stream Double)
v = (vx, vy)
-- | The relative x position between ownship and the intruder.
sx :: Stream Double
sx = extern "relative_position_x" Nothing
-- | The relative y position between ownship and the intruder.
sy :: Stream Double
sy = extern "relative_position_y" Nothing
-- | The relative z position between ownship and the intruder.
sz :: Stream Double
sz = extern "relative_position_z" Nothing
-- | The relative position as a 2D vector.
s :: (Stream Double, Stream Double)
s = (sx, sy)
-- The following section contains basic libraries for working with vectors.
-- | Multiply two Vectors.
(|*|) :: Vect2 -> Vect2 -> Stream Double
(|*|) (x1, y1) (x2, y2) = (x1 * x2) + (y1 * y2)
-- | Calculate the square of a vector.
sq :: Vect2 -> Stream Double
sq x = x |*| x
-- | Calculate the length of a vector.
norm :: Vect2 -> Stream Double
norm = sqrt . sq
-- | Calculate the determinant of two vectors.
det :: Vect2 -> Vect2 -> Stream Double
det (x1, y1) (x2, y2) = (x1 * y2) - (x2 * y1)
-- | Compare two vectors, taking into account the small error that is
-- introduced by the usage of `Double`s.
(~=) :: Stream Double -> Stream Double -> Stream Bool
a ~= b = (abs (a - b)) < 0.001
-- | Negate a vector.
neg :: Vect2 -> Vect2
neg (x, y) = (negate x, negate y)
-- From here on the algorithm, as described by the paper mentioned on the top
-- of this file, is implemented. Please refer to the paper for details.
tau :: Vect2 -> Vect2 -> Stream Double
tau s v = if s |*| v < 0
then (-(sq s)) / (s |*| v)
else -1
tcpa :: Vect2 -> Vect2 -> Stream Double
tcpa s v@(vx, vy) = if vx ~= 0 && vy ~= 0
then 0
else -(s |*| v)/(sq v)
taumod :: Vect2 -> Vect2 -> Stream Double
taumod s v = if s |*| v < 0
then (dthr * dthr - (sq s))/(s |*| v)
else -1
tep :: Vect2 -> Vect2 -> Stream Double
tep s v = if (s |*| v < 0) && ((delta s v dthr) >= 0)
then theta s v dthr (-1)
else -1
delta :: Vect2 -> Vect2 -> Stream Double -> Stream Double
delta s v d = (d*d) * (sq v) - ((det s v)*(det s v))
-- Here the formula says : (s . orth v)^2 which is the same as det(s,v)^2
theta :: Vect2 -> Vect2 -> Stream Double -> Stream Double -> Stream Double
theta s v d e = (-(s |*| v) + e * (sqrt $ delta s v d)) / (sq v)
tcoa :: Stream Double -> Stream Double -> Stream Double
tcoa sz vz = if (sz * vz) < 0
then (-sz) / vz
else -1
dcpa :: Vect2 -> Vect2 -> Stream Double
dcpa s@(sx, sy) v@(vx, vy) = norm (sx + (tcpa s v) * vx, sy + (tcpa s v) * vy)
-- Well clear Violation --
-- | Determines if the well clear property is violated or not.
wcv :: (Vect2 -> Vect2 -> Stream Double) ->
Vect2 -> Stream Double ->
Vect2 -> Stream Double ->
Stream Bool
wcv tvar s sz v vz = (horizontalWCV tvar s v) && (verticalWCV sz vz)
verticalWCV :: Stream Double -> Stream Double -> Stream Bool
verticalWCV sz vz =
((abs $ sz) <= zthr) ||
(0 <= (tcoa sz vz) && (tcoa sz vz) <= tcoathr)
horizontalWCV :: (Vect2 -> Vect2 -> Stream Double) -> Vect2 -> Vect2 -> Stream Bool
horizontalWCV tvar s v =
(norm s <= dthr) ||
(((dcpa s v) <= dthr) && (0 <= (tvar s v)) && ((tvar s v) <= tthr))
spec = do
Monad.void $ prop "1a" (forall $ (tau s v) ~= (tau (neg s) (neg v)))
-- Monad.void $ prop "3d" (forall $ (wcv tep s sz v vz) == (wcv tep (neg s) (-sz) (neg v) (-vz)))
main :: IO ()
main = do
spec' <- reify spec
-- Use Z3 to prove the properties.
results <- CT.prove CT.Z3 spec'
-- Print the results.
forM_ results $ \(nm, res) -> do
putStr $ nm <> ": "
case res of
CT.Valid -> putStrLn "valid"
CT.Invalid -> putStrLn "invalid"
CT.Unknown -> putStrLn "unknown"
|
