-
Notifications
You must be signed in to change notification settings - Fork 13
/
RoMEDiffEqExt.jl
63 lines (53 loc) · 1.6 KB
/
RoMEDiffEqExt.jl
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
module RoMEDiffEqExt
using DifferentialEquations
using Manifolds
using Dates, TimeZones
using Interpolations
using TensorCast
# using RoME
import RoME: imuKinematic!, RotVelPos, InertialDynamic
import RoME: getPointIdentity
import IncrementalInference: DERelative
function InertialDynamic(
tspan::Tuple{<:Real, <:Real}, # = _calcTimespan(Xi),
dt::Real,
gyros::AbstractVector,
accels::AbstractVector;
N::Integer = size(gyros,1),
timestamps = collect(range(tspan[1]; step=dt, length=N)),
)
# use data interpolation
gyros_t = linear_interpolation(timestamps, gyros)
accels_t = linear_interpolation(timestamps, accels)
data = Ref((gyro=gyros_t, accel=accels_t))
domain = RotVelPos
state0 = allocate(getPointIdentity(domain))
state1 = allocate(getPointIdentity(domain))
problemType = ODEProblem
# forward time problem
fproblem = problemType(imuKinematic!, state0, tspan, data; dt)
# backward time problem
bproblem = problemType(imuKinematic!, state1, (tspan[2], tspan[1]), data; dt = -dt)
# build the IIF recognizable object
return DERelative(domain, fproblem, bproblem, data)
end
# function InertialDynamic(
# tspan::Tuple{<:Real, <:Real}, # = _calcTimespan(Xi),
# dt::Real,
# gyros::AbstractVector,
# accels::AbstractVector;
# kw...
# )
# @cast gyros_[i,d] := gyros[i][d]
# @cast accels_[i,d] := accels[i][d]
# InertialDynamic(tspan,dt,gyros_,accels_;kw...)
# end
function InertialDynamic(
tspan::Tuple{<:ZonedDateTime, <:ZonedDateTime},
w...;
kw...
)
tspan_ = map(t -> datetime2unix(DateTime(t)), tspan)
InertialDynamic(tspan_, w...; kw...)
end
end # weakmod