Initializing an inertial sensor using soft constraints and penalty functions

A method for determining an estimate of the initial orientation Q0 and initial velocity v0 of an inertial sensor, comprising the steps:

• collecting sample data from an inertial sensor over a capture time interval [ti, tf] that includes an activity of interest, said sample data including accelerometer data s(t) and rate gyro data w(t);
• selecting an initialization interval [t0, tn]⊂[ti, tf] within said capture time interval;
• defining a set of soft constraints {Ci} on the position r(t) and velocity v(t) of said inertial sensor during said initialization interval, where each Ci⊂{r:[t0, tn]→3}×{v:
• [t0, tn]→3}, where said soft constraints represent approximations of typical patterns of position and velocity trajectories for said activity of interest during said initialization interval;
• defining penalty metrics mj(r(t), v(t)) to quantify the deviation of calculated trajectories of said position r(t) and said velocity v(t) from said soft constraints {Ci};
• and
• calculating estimates of the initial orientation Q0=Q(t0) and initial velocity v0=v(t0) of said inertial sensor at the start of said initialization interval to minimize said penalty metrics when said initial orientation and initial velocity are used as initial conditions to integrate said sample data into orientation, position and velocity over said initialization interval via the coupled differential equations
• ⅆ
• Q
• ⅆ
• t
• =
• Q
• ⁢
• ⁢
• ω
• x
• ,
• ⅆ
• v
• ⅆ
• t
• =
• Qs
• +
• g
• ,
• ⅆ
• r
• ⅆ
• t
• =
• v
• .