From 8b2b6be02d5d1aea32d1e4cee47fcd998b86f111 Mon Sep 17 00:00:00 2001 From: oloft Date: Sat, 26 Aug 2017 16:19:46 +0200 Subject: [PATCH] Working optimization and some tests. --- src/dev.jl | 393 +++++++++++++++++++++++++++++++ src/tst.jl | 31 +++ src/types/PiecewiseQuadratic.jl | 156 ++++++++++-- src/types/QuadraticForm2.jl | 14 ++ src/types/QuadraticPolynomial.jl | 69 +++++- test/piecewise_quadratics.jl | 74 ++++++ test/quadratic_forms.jl | 46 ++++ test/quadratic_polynomials.jl | 21 ++ test/runtests.jl | 8 +- test/transition_costs.jl | 32 +++ test/tst.jl | 65 +++++ 11 files changed, 878 insertions(+), 31 deletions(-) create mode 100644 src/dev.jl create mode 100644 src/tst.jl create mode 100644 src/types/QuadraticForm2.jl create mode 100644 test/piecewise_quadratics.jl create mode 100644 test/quadratic_forms.jl create mode 100644 test/quadratic_polynomials.jl create mode 100644 test/transition_costs.jl create mode 100644 test/tst.jl diff --git a/src/dev.jl b/src/dev.jl new file mode 100644 index 0000000..bcb3735 --- /dev/null +++ b/src/dev.jl @@ -0,0 +1,393 @@ + +module dev + +import Base.- +import Base.+ +import Base.show +import Base.Operators +import Base.start +import Base.next +import Base.done +import Base.length + +import Base.== + +import IterTools + +using StaticArrays +using PyPlot +using Polynomials +using QuadGK + + +include("types/QuadraticPolynomial.jl") +include("types/PiecewiseQuadratic.jl") +include("types/QuadraticForm2.jl") + + + + +function roots(p::QuadraticPolynomial) + quad_expr = p.b^2 - 4*p.a*p.c + if quad_expr < 0 + return NaN, NaN + end + + if p.a != 0 + term1 = (p.b / 2 / p.a) + term2 = sqrt(quad_expr) / 2 / abs(p.a) + return -term1-term2, -term1+term2 + else + term = -p.c / p.b + return term, Inf + end +end + + + + + + +# TODO come up with better symbol for ρ +""" +Inserts a quadratic polynomial ρ into the linked list Λ which represents a piecewise quadratic polynomial +""" +function add_quadratic{T}(Λ::PiecewiseQuadratic{T}, ρ::QuadraticPolynomial{T}) + + if isnull(Λ.next) + insert(Λ, ρ, -1e9) + return + end + + + λ_prev = Λ + λ_curr = Λ.next + + while ~isnull(λ_curr) + + left_endpoint = get(λ_curr).left_endpoint + + # TODO: This is probably not needed now.. + if left_endpoint == -1e9 + #println("minus-inf") + left_endpoint = -10000 + end + + right_endpoint = get_right_endpoint(get(λ_curr)) + + if right_endpoint == 1e9 + #println("inf") + right_endpoint = left_endpoint + 20000 + end + + Δ = ρ - get(λ_curr).p + + root1,root2 = roots(Δ) + + #println(root1, " : ", root2, " .... (", (left_endpoint, right_endpoint)) + + if root1 > left_endpoint && root1 < right_endpoint + #println("case 1:") + λ_prev, λ_curr = impose_quadratic_to_root(λ_prev, get(λ_curr), root1, (left_endpoint+root1)/2, ρ, Δ) + #println(Λ) + end + + if isnull(λ_curr) + break + end + + if root2 > left_endpoint && root2 < right_endpoint + #println("case 2:") + λ_prev, λ_curr = impose_quadratic_to_root(λ_prev, get(λ_curr), root2, (root1 + root2)/2, ρ, Δ) + #println(Λ) + if Δ.a > 0; return; end # Saves perhaps 5% of computation time + end + + if isnull(λ_curr) + break + end + + #println("case 3:") + + λ_prev, λ_curr = impose_quadratic_to_endpoint(λ_prev, get(λ_curr), (get(λ_curr).left_endpoint + right_endpoint)/2, ρ, Δ) + #println(Λ) + + if isnull(λ_curr) + break + end + end + +end + + + +# TODO Possibly just insert all roots, and then do the comparisons? +# Leads to some unnecessary nodes being created but is perhaps simplier? + +function impose_quadratic_to_root(λ_prev, λ_curr, root, midpoint, ρ, Δ) + if Δ((λ_curr.left_endpoint + root)/2) < 0 # ρ is smallest, i.e., should be inserted + if λ_prev.p === ρ + #println("1") + λ_curr.left_endpoint = root + return λ_prev, Nullable{PiecewiseQuadratic}(λ_curr) + else + #println("2") + + #λ_new = insert(λ_prev, ρ, λ_curr.left_endpoint) + #λ_curr.left_endpoint = root + #return λ_new, λ_curr + + λ_new = insert(λ_curr, λ_curr.p, root) + λ_curr.p = ρ + return λ_curr, λ_new + end + else + #println("3") + λ_new = insert(λ_curr, λ_curr.p, root) # insert new interval, but defer deciding on wheather λ_curr.p or ρ is smallest + return λ_curr, λ_new + end +end + +function impose_quadratic_to_endpoint(λ_prev, λ_curr, midpoint, ρ, Δ) + if Δ(midpoint) < 0 # ρ is smallest, i.e., should be inserted + if λ_prev.p === ρ + #println("1") + λ_new = delete_next(λ_prev) + return λ_prev, λ_new + else + #println("2") + λ_curr.p = ρ + return λ_curr, λ_curr.next + end + else + # Do nothing + #println("3") + return λ_curr, λ_curr.next + end +end + + + + + +function minimize_wrt_x2(qf::QuadraticForm2) + P = qf.P + q = qf.q + r = qf.r + + if P[2,2] > 0 + QuadraticPolynomial(P[1,1] - P[1,2]^2 / P[2,2], + q[1] - P[1,2]*q[2] / P[2,2], + r - q[2]^2 / P[2,2]/ 4) + elseif P[2,2] == 0 || P[1,2] == 0 || q[2] == 0 + QuadraticPolynomial(P[1,1], q[1], r) + else + # There are some special cases, but disregards these + QuadraticPolynomial(0,0,-Inf) + end +end + + +""" +Find optimal fit +""" +function find_optimal_fit(Λ_0, ℓ, M) + + N = size(ℓ, 2) + + Λ = Array{PiecewiseQuadratic}(M, N-1) + + Λ[1, :] .= Λ_0 + + for m=2:M + for i=N-m+1:-1:1 + Λ_new = create_new_pwq() + for ip=i+1:N-m + + for λ in Λ[m-1, ip] + p = λ.p + + ρ = dev.minimize_wrt_x2( + ℓ[i,ip] + dev.QuadraticForm2(@SMatrix([0 0; 0 1])*p.a, @SVector([0, 1])*p.b, p.c)) + ρ.time_index = ip + ρ.ancestor = p + + + dev.add_quadratic(Λ_new, ρ) + end + end + Λ[m, i] = Λ_new + end + end + return Λ +end + + +# Finds the minimum of a positive definite quadratic one variable polynomial +# the find_minimum fcn returns (opt_x, opt_val) +function find_minimum(p::QuadraticPolynomial) + if p.a <= 0 + println("No unique minimum exists") + return (NaN, NaN) + else + x_opt = -p.b / 2 / p.a + f_opt = -p.b^2/4/p.a + p.c + return (x_opt, f_opt) + end +end + + +function find_minimum(Λ::PiecewiseQuadratic) + # May assume that the minimum is at the staionary point of the polynomial + # TODO: True? + + f_opt = Inf + p_opt = Λ.p # The segment containing the smallest polynimal + x_opt = NaN + + for λ in Λ + x, f = find_minimum(λ.p) + if f < f_opt + f_opt = f + x_opt = x + p_opt = λ.p + end + end + return p_opt, x_opt, f_opt +end + + +function recover_solution(Λ::PiecewiseQuadratic, first_index=1, last_index=-1) + + p, y, f = find_minimum(Λ) + + I = [first_index] + + while true + push!(I, p.time_index) + + if isnull(p.ancestor); + break; + end + + p = get(p.ancestor) + end + + if last_index != -1 + I[end] = last_index + end + + return I, y, f +end + + + +# TODO Maybe use big T for time indices, howabout mathcal{T} +""" +Computes the transition costs ℓ given a +polynomial and a sequence t +""" +function compute_transition_costs(g, t::AbstractArray) + + # Find primitive functions to g, t*g, and g^2 + # and evaluate them at the break points + #I_g = polyint(g).(t) + #I_g2 = polyint(g^2).(t) + #I_tg = polyint(Poly([0,1]) * g).(t) + + N = length(t) + + # Find primitive functions to g, t*g, and g^2 at the break points + I_g = zeros(size(t)) + I_g2 = zeros(size(t)) + I_tg = zeros(size(t)) + + for i=2:N + I_g[i] = I_g[i-1] + quadgk(g, t[i-1], t[i])[1] + I_g2[i] = I_g2[i-1] + quadgk(t -> g(t)^2, t[i-1], t[i])[1] + I_tg[i] = I_tg[i-1] + quadgk(t -> t*g(t), t[i-1], t[i])[1] + end + + + ℓ = Array{QuadraticForm2}(N-1,N) + + for i=1:N-1 + for ip=i+1:N + + P = (t[ip] - t[i]) * @SMatrix [1/3 1/6; 1/6 1/3] + + q = -2* 1/(t[ip]-t[i]) * + @SVector [-(I_tg[ip] - I_tg[i]) + t[ip]*(I_g[ip] - I_g[i]), + (I_tg[ip] - I_tg[i]) - t[i]*(I_g[ip] - I_g[i])] + + r = I_g2[ip] - I_g2[i] + + ℓ[i,ip] = QuadraticForm2(P, q, r) + end + end + + return ℓ +end + +""" +Evaluate the optimal cost (using least squares) for all +possible index sets with K elements +""" +function brute_force_optimization(ℓ, K) + cost_best = Inf + + I_best = [] + Y_best = [] + + N = size(ℓ, 2) + + for I=IterTools.subsets(2:N-1, K) + + I = [1; I; N] + P = zeros(K+2, K+2) + q = zeros(K+2) + r = 0 + + # Form quadratic cost function Y'*P*Y + q'*Y + r + # corresponding to the y-values in the vector Y + for j=1:K+1 + P[j:j+1,j:j+1] .+= ℓ[I[j], I[j+1]].P + q[j:j+1] .+= ℓ[I[j], I[j+1]].q + r += ℓ[I[j], I[j+1]].r + end + + # find the optimal Y-vector, and compute the correspinding error + Yopt = -(P \ q) / 2 + cost = Yopt' * P * Yopt + q' * Yopt + r + + if cost < cost_best + cost_best = cost + Y_best = Yopt + I_best = I + end + end + return I_best, Y_best, cost_best +end + +function find_optimal_y_values(ℓ, I) + P = zeros(length(I), length(I)) + q = zeros(length(I)) + r = 0 + + # Form quadratic cost function Y'*P*Y + q'*Y + r + # corresponding to the y-values in the vector Y + for j=1:length(I)-1 + P[j:j+1,j:j+1] .+= ℓ[I[j], I[j+1]].P + q[j:j+1] .+= ℓ[I[j], I[j+1]].q + r += ℓ[I[j], I[j+1]].r + end + + # find the optimal Y-vector, and compute the correspinding error + Y = -(P \ q) / 2 + f = Y' * P * Y + q' * Y + r + return Y, f +end + + +# end of module +end diff --git a/src/tst.jl b/src/tst.jl new file mode 100644 index 0000000..593d0d2 --- /dev/null +++ b/src/tst.jl @@ -0,0 +1,31 @@ +# PiecewiseLinearContinuousFitting +# Piecewise Linear Continouous Interpolation Constraints +# Sparse L2 Optimal Fitting subject Continuity Constraints +# Integrated Square + +include("datatypes.jl") + +using Polynomials +using IterTools + +t = linspace(0,1,30) + + + + +g = Poly([0,0,1,-1]) + +ℓ = dev.compute_transition_costs(g, t) + + + +[I, Y] = brute_force_optimization(ℓ, K) + + +x = linspace(0,1,100) + +#closefig() + +using PyPlot +figure(1) +plot(x, g(x), "g", t[I], Y, "r") diff --git a/src/types/PiecewiseQuadratic.jl b/src/types/PiecewiseQuadratic.jl index cede804..813be1d 100644 --- a/src/types/PiecewiseQuadratic.jl +++ b/src/types/PiecewiseQuadratic.jl @@ -1,28 +1,148 @@ -struct QuadraticOnInterval{T} - p::QuadraticPolynomial{T} - endpoint::T +# Note that the first element of the list should be interpreted as the list head +# Should perhaps be reconsidered +# +# Also note that left and right endpoints are represented with ±1e9, +# in order to allow comparison of polynomials far to the left/right, +# Could also just look at the sign of Δ.a +mutable struct PiecewiseQuadratic{T} +p::QuadraticPolynomial{T} +left_endpoint::Float64 +next::Nullable{PiecewiseQuadratic{T}} end -struct PiecewiseQuadratic{T} - qlist::LList{QuadraticOnInterval{T}} +""" +Constructs piece of quadratic polynomial poiting to NULL, i.e. +a rightmost piece, or one that has not been inserted into a piecewise quadfatic +""" +function PiecewiseQuadratic{T}(p::QuadraticPolynomial{T}, left_endpoint) + PiecewiseQuadratic(p, left_endpoint, Nullable{PiecewiseQuadratic{T}}()) +end +function PiecewiseQuadratic{T}(p::QuadraticPolynomial{T}, left_endpoint, next::PiecewiseQuadratic{T}) + PiecewiseQuadratic{T}(p, left_endpoint, next) +end + +start{T}(pwq::PiecewiseQuadratic{T}) = pwq.next +done{T}(pwq::PiecewiseQuadratic{T}, iterstate::Nullable{PiecewiseQuadratic{T}}) = isnull(iterstate) +next{T}(pwq::PiecewiseQuadratic{T}, iterstate::Nullable{PiecewiseQuadratic{T}}) = (get(iterstate), get(iterstate).next) + +function insert{T}(pwq::PiecewiseQuadratic{T}, p::QuadraticPolynomial, left_endpoint) + pwq.next = PiecewiseQuadratic(p, left_endpoint, pwq.next) + return pwq.next +end + +# Delete the node after pwq and return the node that will follow after +# pwq after the deletion +function delete_next{T}(pwq::PiecewiseQuadratic{T}) + pwq.next = get(pwq.next).next + return pwq.next +end + +function get_right_endpoint(λ::PiecewiseQuadratic) + if isnull(λ.next) + return 1e9 + else + return get(λ.next).left_endpoint + end +end + + +function length(pwq::PiecewiseQuadratic) + n = 0 + for x in pwq # Skip the dummy head element + n += 1 + end + return n +end + + +function show(io::IO, Λ::PiecewiseQuadratic) + #@printf("PiecewiseQuadratic (%i elems):\n", length(Λ)) + + for λ in Λ + if λ.left_endpoint == -1e9 + @printf("[ -∞ ,") + else + @printf("[%3.2f,", λ.left_endpoint) + end + + if get_right_endpoint(λ) == 1e9 + @printf(" ∞ ]") + else + @printf(" %3.2f]", get_right_endpoint(λ)) + end + + @printf("\t : ") + show(λ.p) + @printf("\n") + end +end + + +function evalPwq(Λ::PiecewiseQuadratic, x) + y = zeros(x) + for λ in Λ + inds = λ.left_endpoint .<= x .< get_right_endpoint(λ) + y[inds] .= λ.p.(x[inds]) + end + return y end -PiecewiseQuadratic{T}(p::QuadraticPolynomial{T}) = PiecewiseQuadratic{T}(llist(QuadraticOnInterval(p,typemax(T)))) + +function plot_pwq(Λ::PiecewiseQuadratic, plot_style="-") + figure() + + for λ in Λ + left_endpoint = λ.left_endpoint + right_endpoint = get_right_endpoint(λ) + + if left_endpoint == -1e9 + left_endpoint = right_endpoint - 2.0 + end + + if right_endpoint == 1e9 + right_endpoint = left_endpoint + 2.0 + end + + y_grid_gray = linspace(left_endpoint-1, right_endpoint+1) + plot(y_grid_gray, polyval.(λ.p, y_grid_gray), "gray") + + y_grid = linspace(left_endpoint, right_endpoint) + plot(y_grid, polyval.(λ.p, y_grid), "red") + + end +end """ - min!(q1::PiecewiseQuadratic{T}, q2::QuadraticOnInterval{T}) -Update `q1(x)` on the interval `I` in `q2` to be `q1(x) := min(q1(x),q2(x)) ∀ x∈I` +Creates empty piecewise-quadratic polynomial consisting only of the list head +""" +function create_new_pwq() + return PiecewiseQuadratic(QuadraticPolynomial([NaN,NaN,NaN]), NaN, Nullable{PiecewiseQuadratic{Float64}}()) +end + +""" +Creates piecewise-quadratic polynomial containing one element """ -function min!{T}(q1::PiecewiseQuadratic{T}, q2::QuadraticOnInterval{T}) - #TODO +function create_new_pwq(p::QuadraticPolynomial) + pwq = PiecewiseQuadratic(p, -1e9, Nullable{PiecewiseQuadratic{Float64}}()) + return PiecewiseQuadratic(QuadraticPolynomial([NaN,NaN,NaN]), NaN, pwq) +end + +function generate_PiecewiseQuadratic(args) + return PiecewiseQuadratic(QuadraticPolynomial([NaN,NaN,NaN]), NaN, _generate_PiecewiseQuadratic_helper(args)) +end + +function _generate_PiecewiseQuadratic_helper(args) + if Base.length(args) > 1 + return PiecewiseQuadratic(QuadraticPolynomial(args[1][1]), args[1][2], _generate_PiecewiseQuadratic_helper(args[2:end])) + else + return PiecewiseQuadratic(QuadraticPolynomial(args[1][1]), args[1][2]) + end end -Λs = Array{PiecewiseQuadratic,1}(len1,len2) -N = 10 -M = 4 -for m = M:-1:1 - for i = (N-m):-1:1 - Λim = PiecewiseQuadratic(QuadraticPolynomial(Inf,Inf,Inf)) - for ip = (i+1):(N-m+1) - update!(Λim, ts[ip], Λ[m,ip], l[i,ip]) + + +""" +min!(q1::PiecewiseQuadratic{T}, q2::QuadraticOnInterval{T}) +Update `q1(x)` on the interval `I` in `q2` to be `q1(x) := min(q1(x),q2(x)) ∀ x∈I` +""" diff --git a/src/types/QuadraticForm2.jl b/src/types/QuadraticForm2.jl new file mode 100644 index 0000000..29ac94d --- /dev/null +++ b/src/types/QuadraticForm2.jl @@ -0,0 +1,14 @@ +# Quadratic form of 2 variables, used for representing the transition costs ℓ +type QuadraticForm2{T} + P::SMatrix{2,2,T} + q::SVector{2,T} + r::T +end + +function (qf::QuadraticForm2)(y, yp) + return [y,yp]'*qf.P*[y,yp] + qf.q'*[y, yp] + qf.r +end + + ++(qf1::QuadraticForm2, qf2::QuadraticForm2) = QuadraticForm2(qf1.P+qf2.P, qf1.q+qf2.q, qf1.r+qf2.r) +==(qf1::QuadraticForm2, qf2::QuadraticForm2) = (qf1.P==qf2.P) && (qf1.q==qf2.q) && (qf1.r==qf2.r) diff --git a/src/types/QuadraticPolynomial.jl b/src/types/QuadraticPolynomial.jl index 0fa19e6..bad6d30 100644 --- a/src/types/QuadraticPolynomial.jl +++ b/src/types/QuadraticPolynomial.jl @@ -1,18 +1,67 @@ -struct QuadraticPolynomial{T<:Real} - a::T - b::T - c::T +# Quadratic Polynomial to represent cost-to-go function, +# keeos track of its "ancestor" to simplify the recovery of the solution +# TODO: penality for making mutable, so ancestor can be set later on, +# or set ancestor at creation +mutable struct QuadraticPolynomial{T<:Real} +a::T +b::T +c::T +ancestor::Nullable{QuadraticPolynomial{T}} +time_index::Int +function QuadraticPolynomial{T}(a::T, b::T, c::T) where {T} + # @assert a ≥ 0, # Δ may have negative a ... + new(a, b, c, Nullable{QuadraticPolynomial{T}}(),-1) end - -function QuadraticPolynomial{T}(a::T,b::T,c::T) +function QuadraticPolynomial{T}(a::T, b::T, c::T,ancestor, time_index) where {T} @assert a ≥ 0 - QuadraticPolynomial{T}(a,b,c) + new(a, b, c, ancestor, time_index) +end +end + +QuadraticPolynomial{T}(a::T,b::T,c::T) = QuadraticPolynomial{T}(a,b,c) + +function QuadraticPolynomial(x::Vector) + QuadraticPolynomial(x[3], x[2], x[1]) +end + +function Base.show(io::IO, p::QuadraticPolynomial) + print(@sprintf("%.2f*x^2 + %.2f*x + %.2f ", p.a, p.b, p.c)) end + + + +-(p1::QuadraticPolynomial, p2::QuadraticPolynomial) = QuadraticPolynomial(p1.a-p2.a, p1.b-p2.b, p1.c-p2.c) + + +==(p1::QuadraticPolynomial, p2::QuadraticPolynomial) = (p1.a==p2.a) && (p1.b==p2.b) && (p1.c==p2.c) + + +polyval(p::QuadraticPolynomial, x) = p.a*x^2 + p.b*x + p.c + +function (p::QuadraticPolynomial)(x) + return polyval(p, x) +end + + + +# Finds the minimum of a positive definite quadratic one variable polynomial +# the find_minimum fcn returns (opt_x, opt_val) +function find_minimum(p::QuadraticPolynomial) + if p.a <= 0 + println("No unique minimum exists") + return (NaN, NaN) + else + return (-p.b / 2 / p.a, -p.b^2/4 + p.c) + end +end + + + """ - n, intersec = intersections{T}(p1::QuadraticPolynomial{T},p2::QuadraticPolynomial{T}) - returns number of intersections `n` and a 2-vector `intersec` containing the intersection points - in the first `n` elements. +n, intersec = intersections{T}(p1::QuadraticPolynomial{T},p2::QuadraticPolynomial{T}) +returns number of intersections `n` and a 2-vector `intersec` containing the intersection points +in the first `n` elements. """ function intersections{T}(p1::QuadraticPolynomial{T},p2::QuadraticPolynomial{T}) a = p1.a-p2.a diff --git a/test/piecewise_quadratics.jl b/test/piecewise_quadratics.jl new file mode 100644 index 0000000..0076355 --- /dev/null +++ b/test/piecewise_quadratics.jl @@ -0,0 +1,74 @@ +using Base.Test +include("../src/dev.jl") + + +pwq = dev.generate_PiecewiseQuadratic([([2.0, 2, 1], -Inf), ([2.0, -2, 1], 0.0)]) + +@test length(pwq) == 2 +dev.add_quadratic(pwq, dev.QuadraticPolynomial([1, 0, 1.0])) + +@test length(pwq) == 3 + + +# Given a matrix with coefficients for quadratic polynomials this function +# constructs the quadratic polynomials and inserts them into a piecewise +# quadratic object +function verify_piecewise_quadratic(c_mat) + + poly_list = [dev.QuadraticPolynomial(c_mat[k, :]) for k in 1:size(c_mat,1)] + + # Insert quadratics into piecewise quadfatic + pwq = dev.create_new_pwq() + for p in poly_list + dev.add_quadratic(pwq, p) + end + + # Check that the intersections between the quadratics are in increasing order + for λ in pwq + @test λ.left_endpoint < dev.get_right_endpoint(λ) + end + + # Check for continuity of the piecewise quadratic + for λ in pwq + if isnull(λ.next) + break + end + x = get(λ.next).left_endpoint + println("Continutiy diff: ", λ.p(x) - get(λ.next).p(x)) + @test λ.p(x) ≈ get(λ.next).p(x) + end + + # Check that the piecewise quadratic is smaller than all the quadratics + # that it was formed by + x_grid = -5:0.1:5 + y_pwq = dev.evalPwq(pwq, x_grid) + + for p in poly_list + @printf("suboptimality diff: %2.3f\n", maximum(y_pwq - p.(x_grid))) + #println(" ", x_grid[ (y_pwq - p.(x_grid)) .> 0]) + @test (y_pwq .<= p.(x_grid)) == trues(length(x_grid)) + end + +end + + +c_mat1 = [ +2.53106 1.59097 1.60448 +2.51349 0.681205 1.60553 +2.09364 0.714858 1.08607 +] +verify_piecewise_quadratic(c_mat1) + +c_mat2 = [ +2.01532 1.59269 1.65765 +2.50071 1.56421 1.53899 +2.02767 0.706143 1.129 +] +verify_piecewise_quadratic(c_mat2) + + +# Random test case +N = 10 +c_mat_rand = [2+rand(N) 2*rand(N) 1+rand(N)] + +verify_piecewise_quadratic(c_mat_rand) diff --git a/test/quadratic_forms.jl b/test/quadratic_forms.jl new file mode 100644 index 0000000..d4deb73 --- /dev/null +++ b/test/quadratic_forms.jl @@ -0,0 +1,46 @@ +using Base.Test + +using StaticArrays + +include("../src/dev.jl") + + +# Addition of quaratic forms +Q = dev.QuadraticForm2(@SMatrix([2.0 0; 0 1]), @SVector([0.0,0]), 1.0) + +dev.QuadraticForm2(@SMatrix([2.0 1; 1 2]), @SVector([1.0,0]), 0.0) + +@test Q == dev.QuadraticForm2(@SMatrix([4.0 1; 1 3]), @SVector([1.0,0]), 1.0) + + + +# Maybe somewhat surprising that the tests pass considering equalities between +# Floats, or is the aritmhetic exact + +# x^2 + y^2 + 1 +Q1 = dev.QuadraticForm2(@SMatrix([1 0; 0 1]), @SVector([0, 0]), 1) +@test dev.minimize_wrt_x2(Q1) == dev.QuadraticPolynomial(1,0,1) +@test Q1(1, 2) == 6.0 + + +# (2x - y + 1)^2 + x^2 - 2x + 1 +# = 5x^2 + y^2 + 2 + 2x - 2y - 4xy +Q2 = dev.QuadraticForm2(@SMatrix([5 -2; -2 1]) , +@SVector([2, -2]), +2) +@test dev.minimize_wrt_x2(Q2) == dev.QuadraticPolynomial(1,-2,1) +@test Q2(1, 2) - 1.0 < 1e-14 + + +# x^2 + 2x + 2 +Q3 = dev.QuadraticForm2(@SMatrix([1 0; 0 0]), +@SVector([2, 0]), +2) +@test dev.minimize_wrt_x2(Q3) == dev.QuadraticPolynomial(1,2,2) + + +# y^2 + 2y + 2 +Q4 = dev.QuadraticForm2(@SMatrix([0 0; 0 1]), +@SVector([0, 2]), +2) + +@test dev.minimize_wrt_x2(Q4) == dev.QuadraticPolynomial(0,0,1) diff --git a/test/quadratic_polynomials.jl b/test/quadratic_polynomials.jl new file mode 100644 index 0000000..4d475d2 --- /dev/null +++ b/test/quadratic_polynomials.jl @@ -0,0 +1,21 @@ +using Base.Test + +include("../src/dev.jl") + +# Subtraction of quadratic polynomials +@test dev.QuadraticPolynomial(4.0, -2.0, 3.0) - dev.QuadraticPolynomial(1.0, 1.0, 2.0) == dev.QuadraticPolynomial(3.0,-3.0,1.0) + + +### Minimization of quadratic polynomials ### + +@test dev.find_minimum(dev.QuadraticPolynomial(1 ,0 ,0)) == (0, 0) +@test dev.find_minimum(dev.QuadraticPolynomial(1 ,0 ,1)) == (0, 1) + +# 2*x*(x+1) + 1 +@test dev.find_minimum(dev.QuadraticPolynomial(2, 2, 1)) == (-0.5, 0.5) + +# (x+1)^2 + 1 +@test dev.find_minimum(dev.QuadraticPolynomial(1, 2, 2)) == (-1, 1) + +# 2*((x+1)^2 + 1) +@test dev.find_minimum(dev.QuadraticPolynomial(2, 4, 4)) == (-1, 2) diff --git a/test/runtests.jl b/test/runtests.jl index 760e6d8..8d73c7c 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,5 +1,7 @@ -using DynamicApproximations +#using DynamicApproximations using Base.Test -# write your own tests here -@test 1 == 2 +include("quadratic_polynomials.jl") +include("quadratic_forms.jl") +include("piecewise_quadratics.jl") +include("transition_costs.jl") diff --git a/test/transition_costs.jl b/test/transition_costs.jl new file mode 100644 index 0000000..64318dd --- /dev/null +++ b/test/transition_costs.jl @@ -0,0 +1,32 @@ +using Base.Test + +include("../src/dev.jl") + +using Polynomials + + +t = [-1; 0; 1] + +g = Poly([0]) +ℓ = dev.compute_transition_costs(g, t) +@test ℓ[1,2].P == 1/6*[2 1; 1 2] +@test ℓ[1,2].q == [0,0] +@test ℓ[1,2].r == 0 + + +g = Poly([1]) +ℓ = dev.compute_transition_costs(g, t) +@test ℓ[1,2].P == 1/6*[2 1; 1 2] +@test ℓ[1,2].q == [-1,-1] +@test ℓ[1,2].r == 1 + + +g = Poly([0,1]) +ℓ = dev.compute_transition_costs(g, t) +@test ℓ[2,3].P == 1/6*[2 1; 1 2] +@test ℓ[2,3].q ≈ [-1/3,-2/3] +@test ℓ[2,3].r ≈ 1/3 +@test ℓ[2,3](0,0) == 1/3 +@test ℓ[2,3](1,1) == 1/3 +@test ℓ[2,3](1,2) ≈ 1 +@test ℓ[2,3](1,0) ≈ 1/3 diff --git a/test/tst.jl b/test/tst.jl new file mode 100644 index 0000000..5931824 --- /dev/null +++ b/test/tst.jl @@ -0,0 +1,65 @@ +# PiecewiseLinearContinuousFitting +# Piecewise Linear Continouous Interpolation Constraints +# Sparse L2 Optimal Fitting subject Continuity Constraints +# Integrated Square + +using Base.Test + +include("../src/dev.jl") + + + +using Polynomials +using IterTools + +N = 120 + +t = linspace(0,2π,N) + + + + +#g = Poly([0,0,1,-1]) +g = sin + +@time ℓ = dev.compute_transition_costs(g, t) + + +K = 4 +@time I, Y, f = dev.brute_force_optimization(ℓ, K-1) + + +Λ_0 = [dev.create_new_pwq(dev.minimize_wrt_x2(ℓ[i, N])) for i in 1:N-1] + +@time Λ = dev.find_optimal_fit(Λ_0, ℓ, 7) + +@time I2, y2, f2 = dev.recover_solution(Λ[3, 1], 1, N) +Y2, _ = dev.find_optimal_y_values(ℓ, I2) + +println("Comparison: ", sqrt(f), " --- ", sqrt(f2)) + + +#find_optimal_y_values + + +l = zeros(size(Λ)) +for i=1:size(l,1) + for j=1:size(l,2) + if isassigned(Λ,i,j) + l[i,j] = length(Λ[i, j]) + end + end +end + + +#x = linspace(0,1,100) + +#closefig() + +using PyPlot +#close() +figure(1) +plot(t, g.(t), "g", t[I2], Y2, "bo-") + +figure(2) +plot(1:length(t)-1, l')