Merge pull request #718 from JuliaRobotics/23Q4/enh/inerdyn

initial tests for InertialDynamic factor

Project.toml

@@ -31,6 +31,7 @@ Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
 TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
+TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 
 [weakdeps]

ext/RoMEDifferentialEquationsExt.jl

@@ -2,18 +2,21 @@ module RoMEDifferentialEquationsExt
 
 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::AbstractMatrix,
-  accels::AbstractMatrix;
+  gyros::AbstractVector,
+  accels::AbstractVector;
   N::Integer = size(gyros,1),
-  timestamps = range(tspan[1]; step=dt, length=N) # range(0; step=dt, length=N)
+  timestamps = collect(range(tspan[1]; step=dt, length=N)),
 )
   # use data interpolation
   gyros_t = linear_interpolation(timestamps, gyros)
@@ -35,5 +38,26 @@ function InertialDynamic(
   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

ext/factors/InertialDynamic.jl

@@ -7,6 +7,8 @@ getManifold(::InertialDynamic) = getManifold(RotVelPos)
 
 
 
+
+
 ## TODO consolidate inside module as RoME.imuKinematic
 function imuKinematic!(du, u, p, t; g=SA[0; 0; 9.81])
   # p is IMU input (assumed [.gyro; .accel])
@@ -30,5 +32,67 @@ function imuKinematic!(du, u, p, t; g=SA[0; 0; 9.81])
   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)
+  # x is a VelPose3 point (assumed ArrayPartition)
+  # u is IMU input (assumed [rate; accel])
+  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
 end

src/canonical/GenerateCommon.jl

@@ -202,6 +202,66 @@ function generateGraph_TwoPoseOdo(; solverParams::SolverParams = SolverParams(),
   return dfg
 end
 
+"""
+    $SIGNATURES
+
+Simulates IMU measurements from body frame rates and desired world frame accelerations.
+"""
+function generateField_InertialMeasurement_RateZ(;
+  dt = 0.01,
+  N = 401,
+  rate = [0.0, 0.0, pi/2],
+  w_R_b = [1. 0 0; 0 1 0; 0 0 1],
+  gravity = [0.0, 0, 0],
+  accel0 = [0.0, 0, 0] + gravity,
+  b_a = SA[0.0, 0, 0], # [0.0, pi/2*10, 0],
+  σ_a = 0.0, # 1e-4, #0.16e-3*9.81  # noise density m/s²/√Hz
+  σ_ω = 0.0, # deg2rad(0.0001),  # noise density rad/√Hz
+)
+  tspan = (0.0, dt*(N-1))
+
+  gn = norm(σ_ω) < 1e-14 ? ()->[0, 0, 0] : ()->rand(MvNormal(diagm(ones(3)*σ_ω^2 * 1/dt)))
+  an = norm(σ_a) < 1e-14 ? ()->[0, 0, 0] : ()->rand(MvNormal(diagm(ones(3)*σ_a^2 * 1/dt)))
+
+  gyros = [rate + gn() for _ = 1:N]
+
+  accels = Vector{typeof(accel0)}()
+  push!(accels, deepcopy(accel0) + an())
+  # accels = [deepcopy(accel0) + an()]
+  M = SpecialOrthogonal(3)
+
+  # b_a = [0.1, 0, 0]
+  for g in gyros[1:end-1]
+    X = hat(M, Identity(M), g)
+    exp!(M, w_R_b, w_R_b, X*dt)
+    push!(accels, (b_a .+ an()) + w_R_b' * accel0)
+  end
+
+  (;tspan,gyros,accels)
+end
+
+
+
+generateField_InertialMeasurement_RateZ_noise(;
+  dt = 0.1,
+  N = 11,
+  rate = [0, 0, 0.001],
+  gravity = SA[0, 0, 9.81],
+  accel0 = [0, 0, -1.0] + gravity,
+  σ_a = 1e-4,             # 0.16e-3*9.81  # noise density m/s²/√Hz
+  σ_ω = deg2rad(0.0001),  # noise density rad/√Hz
+) = generateField_InertialMeasurement_RateZ(;
+  dt, 
+  N, 
+  rate, 
+  gravity, 
+  accel0, 
+  σ_a, 
+  σ_ω
+)
+
+
+
 
 
 #

test/inertial/testIMUDeltaFactor.jl

@@ -16,8 +16,9 @@ M = SpecialOrthogonal(3)
 a = RotationVec(ΔR)
 b = Rotations.AngleAxis(ΔR)
 
-
+##
 @testset "IMUDeltaFactor spot checks" begin
+##
 
 M = IMUDeltaGroup()
 ϵ = identity_element(M)
@@ -146,6 +147,7 @@ p_SE3 = exp_lie(M_SE3, X_SE3)
 @test isapprox(p.x[3], p_SE3.x[1])
 
 ## test factor with rotation around z axis and initial velocity up
+# DUPLICATED IN testInertialDynamic.jl
 dt = 0.1
 σ_a = 1e-4#0.16e-3*9.81  # noise density m/s²/√Hz
 σ_ω = deg2rad(0.0001)  # noise density rad/√Hz
@@ -267,6 +269,7 @@ timestamps = collect(range(0; step=dt, length=N))
 @test Δ.x[2][1] > 0
 @test Δ.x[3][1] > 0
 
+##
 end
 
 ##

test/inertial/testInertialDynamic.jl

@@ -0,0 +1,80 @@
+
+using Test
+using RoME
+using DifferentialEquations
+using Dates, TimeZones
+using StaticArrays
+
+
+##
+@testset "test InertialDynamic factor" begin
+##
+
+
+## test factor with rotation around z axis and initial velocity up
+# DUPLICATED IN testIMUDeltaFactor.jl
+
+dt = 0.1
+N = 11
+
+σ_a = 1e-4 #0.16e-3*9.81  # noise density m/s²/√Hz
+σ_ω = deg2rad(0.0001)  # noise density rad/√Hz
+imu = RoME.generateField_InertialMeasurement_RateZ_noise(; dt, N, σ_a, σ_ω)
+
+tst = now(localzone())
+tsp = tst + Second(imu.tspan[2]-imu.tspan[1])
+tspan = (tst,tsp)
+
+##
+
+fac = RoME.InertialDynamic(
+  tspan,
+  dt,
+  imu.accels,
+  imu.gyros,
+)
+
+## build a basic factor graph
+
+fg = initfg()
+
+addVariable!(fg, :w_P0, RotVelPos; timestamp = tst)
+addVariable!(fg, :w_P1, RotVelPos; timestamp = tsp)
+
+addFactor!(fg, [:w_P0;], 
+  ManifoldPrior(
+    getManifold(RotVelPos),
+    ArrayPartition(
+      SA[ 1.0 0.0 0.0; 
+          0.0 1.0 0.0; 
+          0.0 0.0 1.0], 
+      SA[10.0, 0.0, 0.0], 
+      SA[0.0, 0.0, 0.0]
+    ),
+    MvNormal(diagm(ones(9)*1e-3))
+  )
+)
+
+#
+f1 = addFactor!(fg, [:w_P0; :w_P1], fac; graphinit=false)
+
+##
+
+@test !isInitialized(fg, :w_P1)
+doautoinit!(fg, :w_P0)
+@test isInitialized(fg, :w_P0)
+
+try
+  P1 = approxConvBelief(fg, getLabel(f1), :w_P1)
+catch
+  @error "FIXME first approxConv on InertialDynamic failed!"
+end
+
+P1 = approxConvBelief(fg, getLabel(f1), :w_P1)
+
+
+
+##
+end
+
+##