Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #724 from JuliaRobotics/master
v0.24.1-rc1
- Loading branch information
Showing
17 changed files
with
603 additions
and
597 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,108 @@ | ||
|
|
||
| @kwdef struct InertialDynamic{D<:SamplableBelief} <: AbstractManifoldMinimize | ||
| Z::D | ||
| end | ||
|
|
||
| getManifold(::InertialDynamic) = getManifold(RotVelPos) | ||
|
|
||
|
|
||
|
|
||
|
|
||
|
|
||
| ## TODO consolidate inside module as RoME.imuKinematic | ||
| ## du = f(u, params, t) # then solve ODE | ||
| function imuKinematic!(du, u, p, t; g=SA[0; 0; 9.81]) | ||
| # p is IMU input (assumed [.gyro; .accel]) | ||
| M = SpecialOrthogonal(3) | ||
|
|
||
| R = u.x[1] # i_R_b = w_R_b Rotation | ||
| V = u.x[2] # Velocity | ||
| # P = u.x[3] # Position unused here | ||
| # ω_b = u.x[4] # Gyroscope bias | ||
| # A_b = u.x[5] # Accelerometer bias | ||
|
|
||
| ω_m = p[].gyro(t) | ||
| Ω = hat(M, Identity(M), ω_m) # + ω_b) # b_Ωbi skew symmetric (Lie algebra element) | ||
| Ṙ = R * Ω # w_Ṙ_b = i_Ṙ_b = d/dt R = d/dt exp(Ω*Δt) => Ṙ = exp(ΩΔt)*d(ΩΔt)/dt = exp(ΩΔt)*Ω | ||
|
|
||
| A_m = p[].accel(t) # b_Abi | ||
| V̇ = R * A_m - g # i_V̇ = w_V̇ = R * (A_m + A_b) - g | ||
| Ṗ = V # i_Ṗ = w_Ṗ | ||
|
|
||
| du.x[1] .= Ṙ | ||
| du.x[2] .= V̇ | ||
| du.x[3] .= Ṗ | ||
|
|
||
| nothing | ||
| end | ||
|
|
||
|
|
||
|
|
||
| ## ========================================================== | ||
| ## LEGACY BELOW | ||
|
|
||
|
|
||
| # a user specified ODE in standard form | ||
| # inplace `xdot = f(x, u, t)` | ||
| # if linear, `xdot = F*x(t) + G*u(t)` | ||
| function insKinematic!(dstate, state, u, t; i_G = SA[0; 0; -9.81]) | ||
| # x is a VelPose3 point (assumed ArrayPartition) | ||
| # u is IMU input (assumed [rate; accel]) | ||
|
|
||
| # switch variable order for legacy case | ||
| dstate_ = ArrayPartition(dstate.x[2],dstate.x[3],dstate.x[1]) | ||
| state_ = ArrayPartition(state.x[2],state.x[3],state.x[1]) | ||
| # Using robotics frame fwd-std-dwn <==> North-East-Down | ||
| # ODE cross check taken from Farrell 2008, section 11.2.1, p.388 | ||
| # NOTE, Farrell 2008 has gravity logic flipped in some of his equations. | ||
| # WE TAKE gravity as up is positive (think a pendulum hanging back during acceleration) | ||
| # expected gravity in FSD frame (akin to NED). This is a model of gravity we expect to measure. | ||
| return imuKinematic!(dstate_, state_, u, t; g = i_G) | ||
| end | ||
| # Mt = TranslationGroup(3) | ||
| # Mr = SpecialOrthogonal(3) | ||
| # # convention | ||
| # # b is real time body | ||
| # # b1 is one discete timestep in the past, equivalent to `r_Sk = r_Sk1 + r_dSk := r_S{k-1} + r_dSk` | ||
|
|
||
| # # Using robotics frame fwd-std-dwn <==> North-East-Down | ||
| # # ODE cross check taken from Farrell 2008, section 11.2.1, p.388 | ||
| # # NOTE, Farrell 2008 has gravity logic flipped in some of his equations. | ||
| # # WE TAKE gravity as up is positive (think a pendulum hanging back during acceleration) | ||
| # # expected gravity in FSD frame (akin to NED). This is a model of gravity we expect to measure. | ||
| # i_G = [0; 0; -9.81] | ||
|
|
||
| # # attitude computer | ||
| # w_R_b = state.x[2] # Rotation element | ||
| # i_R_b = w_R_b | ||
| # # assume body-frame := imu-frame | ||
| # b_Ωbi = hat(Mr, Identity(Mr), u[].gyro(t)) # so(3): skew symmetric Lie algebra element | ||
| # # assume perfect measurement, i.e. `i` here means measured against native inertial (no coriolis, transport rate, error corrections) | ||
| # i_Ṙ_b = i_R_b * b_Ωbi | ||
| # # assume world-frame := inertial-frame | ||
| # w_Ṙ_b = i_Ṙ_b | ||
| # # tie back to the ODE solver | ||
|
|
||
| # dstate.x[2] .= w_Ṙ_b | ||
| # # Note closed form post integration result (remember exp is solution to first order diff equation) | ||
| # # w_R_b = exp(Mr, w_R_b1, b_Ωbi) | ||
|
|
||
| # # measured inertial acceleration | ||
| # b_Abi = u[].accel(t) # already a tangent vector | ||
| # # inertial (i.e. world) velocity-dot (accel) by compensating (i.e. removing) expected gravity measurement | ||
| # i_V̇ = i_R_b * b_Abi - i_G | ||
| # # assume world is inertial frame | ||
| # w_V̇ = i_V̇ | ||
| # dstate.x[3] .= w_V̇ # velocity state | ||
|
|
||
| # # position-dot (velocity) | ||
| # w_V = state.x[3] | ||
| # i_V = w_V | ||
| # i_Ṗ = i_V | ||
| # w_Ṗ = i_Ṗ | ||
| # dstate.x[1] .= w_Ṗ | ||
|
|
||
| # # TODO add biases, see RoME.InertialPose | ||
| # # state[4] := gyro bias | ||
| # # state[5] := acce bias | ||
| # nothing |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.