diff --git a/README.md b/README.md
index e580aef689224865ff300ed73611abf0140b6157..4b6ef859ac14810073344a610bc0be359af79a80 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
 # Overview
-This project contains supplemental Matlab code for the article:
+This project contains supplemental code for the article:
 
->M. Thelander Andrén, "Using Radial Basis Functions to Appriximate the LQG-Optimal Event-Based Sampling Policy", 
+>M. Thelander Andrén, "Using Radial Basis Functions to Appriximate the LQG-Optimal Event-Based Sampling Policy",
 Submitted to European Control Conference, 2019
 
 It demonstrates a numerical method using radial basis functions (RBF) to compute
@@ -10,10 +10,28 @@ sampled-data LQG problem. The optimal policy is given by the solution to a stati
 partial differential equation (PDE) with free boundary. The PDE is given by:
 
 ```math
--V(\tilde{x}^\intercal Q\tilde{x} - J + \tilde{x}^\intercal A^\intercal\nabla V + \frac{1}{2}\Delta V) = 0,\quad \forall \tilde{x}, 
+-V(\tilde{x}^\intercal Q\tilde{x} - J + \tilde{x}^\intercal A^\intercal\nabla V + \frac{1}{2}\Delta V) = 0,\quad \forall \tilde{x},
 ```
 ```math
 -V\geq 0, \quad \tilde{x}^\intercal Q\tilde{x} - J + \tilde{x}^\intercal A^\intercal\nabla V + \frac{1}{2}\Delta V\geq 0, \quad \forall \tilde{x}.
 ```
 The solution to this PDE is the relative value function $`V:\tilde{x}\rightarrow\mathbb{R}`$, and the optimal sampling policy is to trigger sampling whenever $`V(\tilde{x})=0`$ holds.
 For more details we refer to our [article](ecc2019_paper.pdf).
+
+# Requirements
+The demo code has been tested for Julia version 0.6, which can be downloaded [here](https://julialang.org/downloads/oldreleases.html). The following packages are required:
+
+        -OSQP
+        -Roots
+        -Plots
+        -PyPlot
+
+For each required package, simply run Pkg.add("package_name") in the Julia REPL to install it.
+
+# Running the Demo Code
+First make sure that you have installed Julia v0.6 and the required packages. Then start a Julia REPL in the directory where the file 'demo.jl' is located. In the Julia REPL, type 'include('demo.jl')' to run the demo.
+
+Note that it may take quite some time to run the demo the first time, since all the required packages are then being compiled.
+
+# Modifying the Demo Code
+We encourage you to open the file 'demo.jl' and modify it to try out different precision of the approximation and different systems. Whenever you have changed the parameters to your liking, simply run 'include('demo.jl')' in the Julia REPL again to see the new result.
diff --git a/demo.jl b/demo.jl
new file mode 100644
index 0000000000000000000000000000000000000000..aab1c95edc185ccff03932ad671d58d821747be6
--- /dev/null
+++ b/demo.jl
@@ -0,0 +1,114 @@
+ENV["JULIA_PKGDIR"] = "/var/tmp/marcus/.julia"
+Pkg.init()
+
+#=
+Written by: Marcus Thelander Andrén, 2018
+
+This script demonstrates how to compute an RBF approximation of the relative
+value function V, which is then used to determine the optimal sampling policy.
+
+Without loss of generality, the system is assumed to be of the form
+Q = diagm([q₁,...,qₙ]) and R = I (the system state can always be transformed
+to achieve this).
+
+For more information, see the related article:
+
+    Marcus Thelander Andrén, "Using Radial Basis Functions to Approximate the
+    LQG-Optimal Event-Based Sampling Policy", Submitted to ECC 2019
+=#
+
+#=
+Include helper functions
+=#
+include("functions/rbf_approx.jl")
+
+#=
+Specify if we should search for the smallest feasible shape parameter,
+if we should validate the solution (only available when A=0),
+and if we should plot the results.
+=#
+validation = true
+plotting = true
+search_c = true
+
+#=
+System Parameters
+=#
+A = [0. 15.;15. 0.]
+Q = diagm([1., 1.])
+J = 1.
+
+#=
+Specify a uniform grid of collocation points,
+and the value of the shape parameter
+=#
+xlimits = [[-1.5, 1.5], [-1.5, 1.5]]
+Ni = [30, 30]
+c = 5.0
+
+#=
+Compute weights, and form the RBF approximation
+=#
+α, x, c = rbf_approx(A, Q, J, xlimits, Ni, c=c, search_c=true)
+Vapprox(xi) = min(dot(α, ga.(fill(xi,(length(x),1)), x, c)), 0.)
+
+#=
+Evaluate the RBF approximation on a uniform grid with nxd×nxd points
+=#
+println("EVALUATING RBF-APPROXIMATION ON GRID...")
+nxd = 100
+grid, Vapprox_eval = grid_eval(Vapprox, xlimits, nxd)
+
+#=
+In the case A = 0 and we wish to validate,
+compute analytic solution and evaluate on grid.
+=#
+validation = (validation && A == zeros(A))
+if validation
+    include("functions/solve_a0.jl")
+    P = elliptic_bound(Q,eye(Q))
+    ρ = (J/trace(P))^2
+    println("EVALUATING TRUE SOLUTION ON GRID...")
+    Vtrue(xi) = -0.25*max(0, 2*sqrt(ρ)-dot(P*xi,xi))^2
+    _,Vtrue_eval = grid_eval(Vtrue, xlimits, nxd)
+    mae = maximum(abs.(Vapprox_eval-Vtrue_eval))
+    println("MAE ON GRID = $mae")
+end
+
+#=
+Plotting
+=#
+if plotting
+    using Plots
+    using Contour
+    pyplot()
+    iso = -5e-4
+    #Compute the approximated optimal policy
+    approx_contours = lines(Contour.contour(grid[1],grid[2],Vapprox_eval,iso))
+    approx_bound_ind = indmax(map(x->length(x.vertices),approx_contours))
+    approx_bound = coordinates(approx_contours[approx_bound_ind])
+
+    #Plot the approximation of the value function and the corresponding policy
+    p1=plot(grid[1],grid[2],Vapprox_eval,seriestype=:surface,cbar=false,
+        grid=true,xlabel="x₁",ylabel="x₂",zlabel="V",xlims=(xlimits[1][1],xlimits[1][2]),
+        ylims=(xlimits[2][1],xlimits[2][2]))
+    plot!(p1,approx_bound[1],approx_bound[2],zeros(length(approx_bound[2])),
+        lw=2.0,lc=:red,label="Approximated Bound")
+    scatter!(p1,map(xi->xi[1],x),map(xi->xi[2],x),Vapprox.(x),label="Collocation Points",
+        markershape=:circle,mc=:red,ms=2)
+    p2=plot(approx_bound[1],approx_bound[2],lw=1.5,lc=:red,label="Approximated Bound",
+        xlims=(xlimits[1][1],xlimits[1][2]),ylims=(xlimits[2][1],xlimits[2][2]),xlabel="x₁",ylabel="x₂")
+    x1dot = A[1,1]*grid[1]+A[1,2]*grid[2]
+    x2dot = A[2,1]*grid[1]+A[2,2]*grid[2]
+    #If validation, plot comparison of true and approximated policy
+    if validation
+        true_contours = lines(Contour.contour(grid[1],grid[2],Vtrue_eval,iso))
+        true_bound_ind = indmax(map(x->length(x.vertices),true_contours))
+        true_bound = coordinates(true_contours[true_bound_ind])
+        plot!(p1,true_bound[2],true_bound[1],zeros(length(true_bound[2])),lw=2.0,
+            lc=:blue,label="True Bound")
+        plot!(p2,true_bound[1],true_bound[2],lw=1.5,lc=:blue,label="True Bound")
+    end
+    p3=plot(p1,p2,layout=(1,2),size=(1200,600))
+    gui()
+end
diff --git a/functions/lcp.jl b/functions/lcp.jl
new file mode 100644
index 0000000000000000000000000000000000000000..fa85c7014ee45ac008a1bbef882d81fbefc08341
--- /dev/null
+++ b/functions/lcp.jl
@@ -0,0 +1,117 @@
+# include("rbfs.jl")
+# include("principal_minors.jl")
+
+"""Function for forming the LCP problem given Gaussian RBF's"""
+function form_gaussian_lcp(x::Array{Array{Float64,1}}, c::Float64,
+    A::Array{Float64,2}, Q::Array{Float64,2}, J::Float64
+    )::Tuple{Array{Float64,2},Array{Float64,2},Array{Float64,2},Array{Float64,2}}
+#=
+Setup RBF's and its derivatives
+=#
+ϕ(xi,xj) = ga(xi, xj, c)
+∇ϕ(xi,xj) = ∇ga(xi, xj, c)
+Δϕ(xi,xj) = Δga(xi, xj, c)
+
+#=
+Form matrices used in LCP:
+=#
+nx = length(x)
+b = zeros(nx, 1)
+Φ = zeros(nx, nx)
+xA∇Φ = zeros(nx, nx)
+d2ΔΦ = zeros(nx, nx)
+xiarr = Array{Array{Float64, 1}}(nx)
+for i=1:nx
+    xi = x[i]
+    fill!(xiarr, xi)
+    b[i] = xi'*Q*xi - J
+    Φ[i,:] = ϕ.(xiarr,x)
+    xA∇Φ[i,:] = [xi'*A'].*∇ϕ.(xiarr,x)
+    d2ΔΦ[i,:] = 0.5 * Δϕ.(xiarr,x)
+end
+AΦ = xA∇Φ + d2ΔΦ
+return -AΦ/Φ, b, -AΦ, Φ
+end
+
+
+"""
+Function for checking if LCP is guaranteed to have a unique solution.
+"""
+function unique_solution_lcp(M::Array{Float64,2})::Bool
+#=
+Check if M is a P-matrix.
+Note: Very costly to compute, O(2^nx),
+so only do it for small problem sizes
+=#
+if size(M,1) <= 10
+    return ispmatrix(M)
+else
+    #Note: This is only a sufficient condition
+    return isposdef(M+M')
+end
+end
+
+
+"""
+Function for checking if LCP is guaranteed to have a unique solution.
+
+This method checks uniqueness using the sufficient condition that A and B
+are both row diagonally dominant, which implies M = Ainv(B) is a P-matrix.
+"""
+function unique_solution_lcp(A::Array{Float64,2},B::Array{Float64,2})::Bool
+return is_row_diagonally_dominant(A) && is_row_diagonally_dominant(B)
+end
+
+
+"""Function for checking if a matrix is row diagonally dominant."""
+function is_row_diagonally_dominant(B::Array{Float64,2})::Bool
+rows = abs.(diag(B)[:,:]) .> sum(abs.(B-diagm(diag(B))),2)
+return sum(rows) == length(rows)
+end
+
+
+"""
+Function using binary search to find the smallest shape parameter c such that
+the LCP is guaranteed to have a unqiue solution.
+"""
+function find_smallest_c(x::Array{Array{Float64,1}}, c_guess::Float64,
+    A::Array{Float64,2}, Q::Array{Float64,2}, J::Float64; ϵ::Float64 = 1e-2)::Float64
+#=
+Define sufficient condition for solvability
+=#
+solvable(c) = unique_solution_lcp(form_gaussian_lcp(x,c,A,Q,J)[1])
+
+#=
+Find bounds on c based on initial guess
+=#
+if solvable(c_guess)
+    cu = c_guess
+    cl = c_guess/2.
+    println("cᵤ = $cu, cₗ = $cl")
+    while solvable(cl)
+        cl = cl/2.
+        println("cᵤ = $cu, cₗ = $cl")
+    end
+else
+    cl = c_guess
+    cu = c_guess*2.
+    println("cᵤ = $cu, cₗ = $cl")
+    while !solvable(cu)
+        cu = cu*2.
+        println("cᵤ = $cu, cₗ = $cl")
+    end
+end
+#=
+Lower and upper bound on c found. Proceed with binary search
+=#
+while cu-cl > ϵ
+    c = (cu+cl)/2.
+    if solvable(c)
+        cu = c
+    else
+        cl = c
+    end
+    println("cᵤ = $cu, cₗ = $cl")
+end
+return cu
+end
diff --git a/functions/rbf_approx.jl b/functions/rbf_approx.jl
new file mode 100644
index 0000000000000000000000000000000000000000..84533b2cf5a20272baf824ce8aaf1f01777f07f9
--- /dev/null
+++ b/functions/rbf_approx.jl
@@ -0,0 +1,94 @@
+include("rbfs.jl")
+include("solve_lcp.jl")
+include("lcp.jl")
+
+"""
+By:  Marcus Thelander Andrén, 2018
+
+Function for numerically finding the function V(x) [Henningsson 2012, s82],
+which determines the optimal reset bound. The idea is to approximate V using
+radial basis functions (RBF's) and posing the problem as a linear complementarity
+problem (LCP).
+
+For a given J, we wish to find V satisfying:
+    min{(xᵀQx - J) + xᵀAᵀ∇V + 1/2Tr(∇²V), -V} = 0
+    s.t (xᵀQx - J) + xᵀAᵀ∇V + 1/2Tr(∇²V) ≧ 0, -V ≧ 0.
+
+See [Henningsson 2012] for more details on the problem. The problem can always
+be re-scaled so that Q = diag(...) and R = I, and we assume that has already
+been done here. See [Marcozzi et.al 2001] for details on using RBF's and LCP.
+"""
+function rbf_approx(A::Array{Float64,2}, Q::Array{Float64,2}, J::Float64,
+    xlimits::Array{Array{Float64,1}}, nxi::Array{Int64,1}; c::Float64=1.,
+    search_c::Bool=false, search_c_ϵ::Float64=1e-2, use_gurobi::Bool=true)
+#=
+Error check
+=#
+problem_dim = size(A,2)
+(size(A) != size(Q) || problem_dim != length(nxi) || problem_dim != length(xlimits)) && error("Error: problem dimension must be the same in all input arguments.")
+
+#=
+Setup of collocation points (uniform grid)
+=#
+println("GENERATING COLLOCATION POINTS...")
+grid = Array{StepRangeLen,1}(problem_dim)
+for d=1:problem_dim
+    xdmin = minimum(xlimits[d])
+    xdmax = maximum(xlimits[d])
+    dxd = (xdmax - xdmin)/(nxi[d]-1)
+    grid[d] = xdmin:dxd:xdmax
+end
+x = collect.(vec(collect(Iterators.product(grid...))))
+#=
+Setup RBF's
+=#
+search_c && println("SEARCHING FOR SMALLEST FEASIBLE C...")
+c = search_c ? find_smallest_c(x, c, A, Q, J, ϵ = search_c_ϵ) : c
+search_c && println("FOUND C = $c")
+println("GENERATING LCP...")
+M,β,_,Φ = form_gaussian_lcp(x, c, A, Q, J)
+
+#=
+Solve LCP
+=#
+!search_c && !unique_solution_lcp(M) && error("Error: Unique solution is not guaranteed for c = $c.")
+println("SOLVING LCP...")
+z = solve_lcp_osqp(M, β)
+println("LCP OBJECTIVE VALUE (should be zero) = ",(z'*M*z + z'*β)[1])
+println("COMPUTING RBF-WEIGHTS...")
+α = -Φ\z
+return α, x, c
+end
+
+"""
+Evaluates a function on a grid. Returns the grid as an array containing
+    ranges for each dimension, and the function values in a matrix.
+"""
+function grid_eval(V::Function, xlimits::Array{Array{Float64,1}}, nxd::Int64)
+    problem_dim = length(xlimits)
+    #=
+    Create grid
+    =#
+    grid = Array{StepRangeLen,1}(problem_dim)
+    for d = 1:problem_dim
+        xdmin = minimum(xlimits[d])
+        xdmax = maximum(xlimits[d])
+        dxd = (xdmax - xdmin)/(nxd-1)
+        grid[d] = xdmin:dxd:xdmax
+
+    end
+    #=
+    Evaluate function on grid
+    =#
+    ind = CartesianRange(tuple(length.(grid)...))
+    val_grid = map(V, getindex.(grid, i.I) for i in ind)
+    #=
+    Permute so that it is compatible with plotting
+    =#
+    if problem_dim == 2
+        val_grid = val_grid'
+    elseif problem_dim == 3
+        val_grid = permutedims(val_grid,[2,1,3])
+    end
+    return grid, val_grid
+end
diff --git a/functions/rbfs.jl b/functions/rbfs.jl
new file mode 100644
index 0000000000000000000000000000000000000000..5a0b900761a2b5c9297d47506cb7e2c2b5ef508d
--- /dev/null
+++ b/functions/rbfs.jl
@@ -0,0 +1,8 @@
+#=
+Defining the Gaussian basis function, and its gradient and Laplacian.
+=#
+
+#Gaussian
+ga(x,xj,c) = exp(-c*norm(x-xj)^2)
+∇ga(x,xj,c) = ga(x,xj,c)*-2.*c*(x-xj)
+Δga(x,xj,c) = ga(x,xj,c)*2.*c*(2.*c*norm(x-xj)^2-length(x))
diff --git a/functions/solve_a0.jl b/functions/solve_a0.jl
new file mode 100644
index 0000000000000000000000000000000000000000..d25735d862dd1f25396c14f6872893c92a940162
--- /dev/null
+++ b/functions/solve_a0.jl
@@ -0,0 +1,38 @@
+#Preliminaries
+using Roots
+
+"""Computes the matrix P>0 for the optimal reset system with A = 0
+for the 2D case.
+P is the solution to the Riccati-like equation:
+
+  PRP + 0.5*Tr(RP)P = Q
+
+The optimal bound for reset is x'Px = 2sqrt(rho), where rho is
+the cost of each reset. See [Toivo, BoB 2012]. The equation is solved by
+diagonalizing the equation and finding the unique root of a scalar function,
+credit to L. Mirkin.
+
+  INPUT ARGUMENTS:
+    Q  : Q > 0, state cost
+    R  : R > 0, noise intensity
+
+  OUTPUT ARGUMENTS:
+    P  : P > 0, solution to the Riccati-like equation
+
+"""
+function elliptic_bound(Q::Array{Float64,2},R::Array{Float64,2})::Array{Float64,2}
+        Cu = chol(Q)
+        By = chol(R)'
+        M = Cu*By*By'*Cu'
+        Σsquare,U = eig(M)
+        n = size(Q,2)
+        fun(x) = (n+4)*x - sum(sqrt.(x^2 + 16*Σsquare))
+        xsol = fzero(fun,0.,1e6)
+        fun(xsol) > 1e-10 && error("No root x could be found for Q = $Q, R = $R.")
+        Qtil = diagm((-xsol/4 + sqrt.(xsol^2/16 +Σsquare))./Σsquare)
+        return Cu'*U*Qtil*U'*Cu
+end
+
+function elliptic_bound(Q::Float64,R::Float64)::Float64
+        return sqrt((2*Q)/(3*R))
+end
diff --git a/functions/solve_lcp.jl b/functions/solve_lcp.jl
new file mode 100644
index 0000000000000000000000000000000000000000..b6658635d10ffad2a7dd6fd4570f105038fd4ed2
--- /dev/null
+++ b/functions/solve_lcp.jl
@@ -0,0 +1,20 @@
+using OSQP
+
+"""
+Function for solving a linear complementarity problem (LCP) of the form
+    zᵀ(Mz + b), s.t z ≧ 0, Mz + b ≧ 0
+where M is a P-matrix. The solver uses the OSQP package.
+"""
+function solve_lcp_osqp(M::Array{Float64}, b::Array{Float64}; max_iter = 1e5,
+    eps_abs = 1e-9, eps_rel = 1e-9)
+nx = size(M,1)
+b = vec(b)
+A = [M; eye(M)]
+c = [b; zeros(nx)]
+model = OSQP.Model()
+OSQP.setup!(model; P=sparse(M+M'), q=b, A=sparse(A), l=-c,
+    u = Inf*ones(2nx), eps_abs=eps_abs, eps_rel=eps_rel,
+    max_iter = max_iter)
+results = OSQP.solve!(model)
+return results.x
+end