A Müller approach for reachability
Luc Jaulin
This microsite is associated to the paper
A Müller approach for reachability
This paper presents an approach to deal with a dynamical system with interval uncertainties.
These uncertainties can be both on the initial state vector, on the time-dependent inputs and on the evolution function.
The approach is based on the Müller theorem often used in an interval context to perform predictions of cooperative systems.
We show here that the Müller method can be used for general non-cooperative systems.
We also show the benefit we can reach by using a conditioning approach in order to reduce the pessimism.
Pendulum
from roblib import *
import codac as co
from codac import IntervalVector,Interval, IntervalMatrix
from mullerlib import γ, numpy2IntervalMatrix,numpy2IntervalVector,Γ,q2xJ,xJ2q,Fe_centered,RK,fq,h
def U(t,eps): return [Interval(-eps**2,eps**2)]*1
def ffx(x,U,t): return IntervalVector([x[1],-co.sin(x[0])-x[1]+U[0]])
def Jffx(x,t): return IntervalMatrix(n,n,[Interval(0),Interval(1),-co.cos(x[0]),Interval(-1)])
def Jf(x,t):
J=Jffx(numpy2IntervalVector(x),t)
return array([J[i][j].mid() for i in range (0,n) for j in range (0,n)]).reshape((n,n))
def fx(x,t):
y=ffx(numpy2IntervalVector(x),ubar(t),t)
return array([y[i].mid() for i in range (0,n)]).reshape((n,1))
def ubar(t): return [Ui.mid() for Ui in U(t,eps)]
def simu_x(x,col):
for k in range(0,kmax):
t=k*dt
x1=x.copy()
x=RK(fx,x,t,dt)
if k%1==0: draw_segment(x1,x,col)
tmax=20
dt=0.01
kmax=int(tmax/dt)
eps=0.01
X0=IntervalVector([[1.0-eps,1.0+eps],[0.2-eps,0.2+eps]])
n=len(X0)
x0=array([X0i.mid() for X0i in X0]).reshape((n,1))
q0=xJ2q(array(X0.mid()).reshape((n,1)),np.eye(n,n))
for x1 in [X0[0].lb(),X0[0].ub()]:
for x2 in [X0[1].lb(),X0[1].ub()]:
simu_x(array([[x1],[x2]]),"yellow")
show_now()
T,Z=[],[]
Ja=np.eye(n,n)
q=q0
_a,_da,_Ja,_dJa=[],[],[],[]
for k in range(0,kmax):
t=k*dt
a,Ja=q2xJ(q)
_a.append(a)
_da.append(fx(a,t))
_Ja.append(Ja)
_dJa.append(Jf(a,t)@Ja)
q=RK(lambda x,t: fq(x,t,fx,Jf),q,t,dt)
z0=γ(X0-X0.mid())
z=z0
for k in range(0,kmax):
t=k*dt
T.append(t)
Z.append(z)
def Fe(E,U,t): return Fe_centered(E,U(t,eps),t,_a[k],_da[k],_Ja[k],_dJa[k],ffx,Jffx)
fz=Γ(Fe)
z=RK(lambda z,t:fz(z,U,t),z,t,dt)
XX=[]
for k in range(0,kmax):
z,a,Ja=Z[k],_a[k],_Ja[k]
Pe=[z[i,0][:,None] for i in [[0,2],[1,2],[1,3],[0,3],[0,2]] ]
X=h(z,a,Ja)
XX.append(X)
if k%20==0:
for i in range (0,4): draw_segment(a+Ja@Pe[i],a+Ja@Pe[i+1],"green",2)
draw_box_border(X[0].lb(),X[0].ub(),X[1].lb(),X[1].ub(),col='blue',w=1)
Dubins car
from roblib import *
import codac as co
from codac import IntervalVector,Interval, IntervalMatrix
from mullerlib import γ, numpy2IntervalMatrix, numpy2IntervalVector, Γ, q2xJ,xJ2q,Fe_centered, RK, fq,h
from collections import deque
def U(t,eps): return [Interval(-eps**2,eps**2)]*4
def c(t,i):
if i==0: return 1.8+0.9*t
else: return 0.4+0.2*t
def ffx(x,U,t):
return IntervalVector( [
x[3]*co.cos(x[2]),
x[3]*co.sin(x[2]),
-(c(t,0)-x[4])*(co.sin(x[2]+U[2])/(x[3]+U[3])) + (c(t,1)-x[5])*(co.cos(x[2]+U[2])/(x[3]+U[3]))+U[0],
(c(t,0)-x[4])*co.cos(x[2]+U[2]) + (c(t,1)-x[5])*co.sin(x[2]+U[2]) -2*(x[3]+U[3])+U[1],
(x[3]+U[3])*co.cos(x[2]+U[2]),
(x[3]+U[3])*co.sin(x[2]+U[2])
])
def fx(x,t):
y=ffx(numpy2IntervalVector(x),uhat(t),t)
return array([y[i].mid() for i in range (0,len(x))]).reshape((len(x),1))
def Jffx(x,t):
x1,x2,x3,x4,x5,x6=x[0],x[1],x[2],x[3],x[4],x[5]
u1,u2,u3,u4=Interval(uhat(t)[0]),Interval(uhat(t)[1]),Interval(uhat(t)[2]),Interval(uhat(t)[3])
df3Idx3=(-(c(t,0)-x5)*co.cos(x3+u3)-(c(t,1)-x6)*(co.sin(x3+u3)))/(x4+u4)
df3Idx4=((c(t,0)-x5)*co.sin(x3+u3)-((c(t,1)-x6)*(co.cos(x3+u3))))/co.sqr(x4+u4)
df3Idx5= co.sin(x3+u3)/(x4+u4)
df3Idx6=-co.cos(x3+u3)/(x4+u4)
df4Idx3=-(c(t,0)-x5)*co.sin(x3+u3) + (c(t,1)-x6)*co.cos(x3+u3)
df4Idx4= Interval(-2)
df4Idx5=-co.cos(x3+u3)
df4Idx6=-co.sin(x3+u3)
return IntervalMatrix(n,n,[
Interval(0),Interval(0),-x4*co.sin(x3),co.cos(x3),Interval(0),Interval(0),
Interval(0),Interval(0), x4*co.cos(x3),co.sin(x3),Interval(0),Interval(0),
Interval(0),Interval(0),df3Idx3,df3Idx4,df3Idx5,df3Idx6,
Interval(0),Interval(0),df4Idx3,df4Idx4,df4Idx5,df4Idx6,
Interval(0),Interval(0),-(x4+u4)*(co.sin(x3+u3)),co.cos(x3+u3),Interval(0),Interval(0),
Interval(0),Interval(0),(x4+u4)*(co.cos(x3+u3)),co.sin(x3+u3),Interval(0),Interval(0)
])
def Jf(x,t):
J=Jffx(numpy2IntervalVector(x),t)
return array([J[i][j].mid() for i in range (0,n) for j in range (0,n)]).reshape((n,n))
def uhat(t): return [Ui.mid() for Ui in U(t,eps)]
def simu_x(x,col):
for k in range(0,kmax):
t=k*dt
x1=x.copy()
x=RK(fx,x,t,dt)
if k%10==0: draw_segment(x1,x,col)
def simu(X0):
n=len(X0)
q0=xJ2q(array(X0.mid()).reshape((n,1)),np.eye(n,n))
simu_x(array(X0.mid()).reshape(n,1),"yellow")
T,Z=[],[]
q=q0
_a,_da,_Ja,_dJa=[],[],[],[]
for k in range(0,kmax):
t=k*dt
a,Ja=q2xJ(q)
_a.append(a)
_da.append(fx(a,t))
_Ja.append(Ja)
_dJa.append(Jf(a,t)@Ja)
q=RK(lambda x,t: fq(x,t,fx,Jf),q,t,dt)
z0=γ(X0-X0.mid())
z=z0
for k in range(0,kmax):
t=k*dt
T.append(t)
Z.append(z)
def Fe(E,U,t): return Fe_centered(E,U,t,_a[k],_da[k],_Ja[k],_dJa[k],ffx,Jffx)
fz=Γ(Fe)
z=RK(lambda z,t:fz(z,U(t,eps),t),z,t,dt)
for k in range(0,kmax):
z,a,Ja=Z[k],_a[k],_Ja[k]
X=h(z,a,Ja)
if k%1==0: XX1.append(X)
if k%100==0: XX2.append(X)
return
tmax=4
dt=0.01
kmax=int(tmax/dt)
eps=0.001
X0=IntervalVector([[-eps,eps],[-eps,eps],[-eps,eps],[1-eps,1+eps],[-eps,eps],[-eps,eps]])
n=len(X0)
ds=0.02
XX1=deque()
XX2=deque()
for x03 in np.arange(-2,2-ds,ds):
X0[2]=Interval(x03,x03+ds)
simu(X0)
for x in XX1:
draw_box_border(x[0].lb(),x[0].ub(),x[1].lb(),x[1].ub(),col='darkblue',w=1)
for x in XX2:
draw_box_border(x[0].lb(),x[0].ub(),x[1].lb(),x[1].ub(),col='red',w=1)
Mullerlib.py
from roblib import *
import codac as co
from codac import IntervalVector,Interval, IntervalMatrix
def numpy2IntervalMatrix(M):
return IntervalMatrix(len(M),len(M), [Interval(v,v) for v in M.flatten()])
def numpy2IntervalVector(v): return IntervalVector(v.flatten())
def inv_γ(z): # intervalize(z) if z=[[1],[2],[4],[5],[7],[8]], returns X=[[1,2],[4,5],[7,8]]
return [[z[i,0],z[i+1,0]] for i in range(0,len(z),2)]
def γ(E): return np.array([v for Ei in E for v in (Ei.lb(),Ei.ub())])[:,None]
def Face(X,i,b): # select the ith face of the box X. b=0 means lowerbound, b=1 means upperbound
Y= [Interval([X[i][0],X[i][1]]) for i in range(0,len(X))]
Y[i]=Interval(X[i][b],X[i][b])
return Y
def Γ(Fe):
def fz(z,U,t):
X=inv_γ(z) #intervalize z into X
z= np.array([])
for i in range(len(X)):
z=append(z,Fe(Face(X,i,0),U,t)[i].lb()) #left face
z=append(z,Fe(Face(X,i,1),U,t)[i].ub()) #right face
z=z.reshape((len(z),1))
return z
return fz
def q2xJ(q): # from q, we return x,J
q=q.flatten()
n=int(np.floor(sqrt(len(q))))
x=q[:n].reshape((n,1))
J=q[-n*n:].reshape((n,n))
return x,J
def xJ2q(x,J): # from x,J, return q
return vstack((x,J.flatten().reshape(-1, 1)))
def centered_form(F,J): return lambda X: F(X.mid())+J(X)*(X-X.mid())
def Fe_centered(E,U,t,a,da,Ja,dJa,ffx,Jffx): #centered form for fe=J^-1*(-dJ*ebar+ffx(Ja*ebar+a,U,t)-da)
def Fe(E): return invJa*(-dJa*E+ffx(Ja*E+a,U,t)-da)
def Je(E): return invJa*(-dJa+Jffx(Ja*E+a,t)*Ja)
E=IntervalVector(E)
Ja,dJa,invJa = map(numpy2IntervalMatrix,(Ja,dJa,inv(Ja)))
a,da=map(numpy2IntervalVector,(a,da))
return centered_form(Fe,Je)(E)
def RK(f,x,t,dt): return x+dt*f(x+(dt/2)*f(x,t),t) # Runge-Kutta
def fq(q,t,fx,Jf):
x,J=q2xJ(q)
dx=fx(x,t)
dJ=(Jf(x,t)@J)
return xJ2q(dx,dJ)
def h(z,a,Ja):
E=IntervalVector(inv_γ(z))
Ja=numpy2IntervalMatrix(Ja)
a=numpy2IntervalVector(a)
return Ja*E+a