<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE article PUBLIC "-//NLM//DTD Journal Publishing DTD v2.3 20070202//EN" "journalpublishing.dtd">
<article xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink" xml:lang="en" article-type="research-article">
<front>
<journal-meta>
<journal-id journal-id-type="nlm-ta">Sensors</journal-id>
<journal-title>Sensors</journal-title>
<issn pub-type="epub">1424-8220</issn>
<publisher>
<publisher-name>Molecular Diversity Preservation International (MDPI)</publisher-name></publisher></journal-meta>
<article-meta>
<article-id pub-id-type="doi">10.3390/s120505310</article-id>
<article-id pub-id-type="publisher-id">sensors-12-05310</article-id>
<article-categories>
<subj-group>
<subject>Article</subject></subj-group></article-categories>
<title-group>
<article-title>Angular Motion Estimation Using Dynamic Models in a Gyro-Free Inertial Measurement Unit</article-title></title-group>
<contrib-group>
<contrib contrib-type="author">
<name><surname>Edwan</surname><given-names>Ezzaldeen</given-names></name><xref ref-type="aff" rid="af1-sensors-12-05310"><sup>1</sup></xref><xref ref-type="corresp" rid="c1-sensors-12-05310"><sup>*</sup></xref></contrib>
<contrib contrib-type="author">
<name><surname>Knedlik</surname><given-names>Stefan</given-names></name><xref ref-type="aff" rid="af2-sensors-12-05310"><sup>2</sup></xref></contrib>
<contrib contrib-type="author">
<name><surname>Loffeld</surname><given-names>Otmar</given-names></name><xref ref-type="aff" rid="af1-sensors-12-05310"><sup>1</sup></xref></contrib></contrib-group>
<aff id="af1-sensors-12-05310">
<label>1</label> Center for Sensor Systems (ZESS), University of Siegen, Paul Bonatz-Str. 9-11, 57068 Siegen, Germany; E-Mail: <email>loffeld@zess.uni-siegen.de</email></aff>
<aff id="af2-sensors-12-05310">
<label>2</label> iMAR GmbH, St. Ingbert, Germany; E-Mail: <email>s.knedlik@imar-navigation.de</email></aff>
<author-notes>
<corresp id="c1-sensors-12-05310">
<label>*</label>Author to whom correspondence should be addressed; E-Mail: <email>edwan@zess.uni-siegen.de</email>; Tel.: +49-271-740-4067.</corresp></author-notes>
<pub-date pub-type="collection">
<year>2012</year></pub-date>
<pub-date pub-type="epub">
<day>26</day>
<month>04</month>
<year>2012</year></pub-date>
<volume>12</volume>
<issue>5</issue>
<fpage>5310</fpage>
<lpage>5327</lpage>
<history>
<date date-type="received">
<day>27</day>
<month>02</month>
<year>2012</year></date>
<date date-type="rev-recd">
<day>27</day>
<month>03</month>
<year>2012</year></date>
<date date-type="accepted">
<day>23</day>
<month>04</month>
<year>2012</year></date></history>
<permissions>
<copyright-statement>© 2012 by the authors; licensee MDPI, Basel, Switzerland</copyright-statement>
<copyright-year>2012</copyright-year>
<license>
<p>This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).</p></license></permissions>
<abstract>
<p>In this paper, we summarize the results of using dynamic models borrowed from tracking theory in describing the time evolution of the state vector to have an estimate of the angular motion in a gyro-free inertial measurement unit (GF-IMU). The GF-IMU is a special type inertial measurement unit (IMU) that uses only a set of accelerometers in inferring the angular motion. Using distributed accelerometers, we get an angular information vector (AIV) composed of angular acceleration and quadratic angular velocity terms. We use a Kalman filter approach to estimate the angular velocity vector since it is not expressed explicitly within the AIV. The bias parameters inherent in the accelerometers measurements' produce a biased AIV and hence the AIV bias parameters are estimated within an augmented state vector. Using dynamic models, the appended bias parameters of the AIV become observable and hence we can have unbiased angular motion estimate. Moreover, a good model is required to extract the maximum amount of information from the observation. Observability analysis is done to determine the conditions for having an observable state space model. For higher grades of accelerometers and under relatively higher sampling frequency, the error of accelerometer measurements is dominated by the noise error. Consequently, simulations are conducted on two models, one has bias parameters appended in the state space model and the other is a reduced model without bias parameters.</p></abstract>
<kwd-group>
<kwd>angular motion estimation</kwd>
<kwd>GF-IMU</kwd>
<kwd>dynamic models</kwd></kwd-group></article-meta></front>
<body>
<sec sec-type="intro">
<label>1.</label>
<title>Introduction</title>
<p>A conventional IMU is composed of three accelerometers and three gyroscopes mounted in a strap-down configuration. Accelerometers are sensors that measure acceleration and gyroscopes are sensors that measure the angular rate of rotation. Gyros are usually corrupted by various sources of errors such as bias instability, noise, scale factor errors… <italic>etc</italic>. For gyros, the bias instability is the dominant error because the inertial system often works as a standalone system. Therefore, inertial systems can be categorized in terms of gyro bias error [<xref ref-type="bibr" rid="b1-sensors-12-05310">1</xref>]. It is shown in [<xref ref-type="bibr" rid="b1-sensors-12-05310">1</xref>,<xref ref-type="bibr" rid="b2-sensors-12-05310">2</xref>] that the biases of the gyros play an important role in causing drift in the position by an example of a biased gyro which causes an error in the position that grows with the cube of time.</p>
<p>The use of distributed accelerometers as an alternative to conventional gyros to infer the angular motion has been a subject of intensive research. Unlike the standard IMU, the GF-IMU uses only accelerometers to infer the acceleration and the angular velocity. It is possible to get the Coriolis acceleration vector, which contains a direct expression of the angular velocity vector, through configurations contain rotating accelerometers. However, the focus of this work is on fixed accelerometer configurations because they are simpler to implement. There are several reasons to use accelerometers for inferring the angular motion. Generally, accelerometers are less costly, less heavy and less power consuming than comparable gyros, which have typically the disadvantage of complicated manufacturing techniques, high cost, high power consumption, high weight, large volume, and limited dynamic range [<xref ref-type="bibr" rid="b3-sensors-12-05310">3</xref>]. A GF-IMU can be used to measure the angular velocities in crashworthiness, sports and motion analysis applications, which are characterized by large peak values of the angular velocity as listed in [<xref ref-type="bibr" rid="b4-sensors-12-05310">4</xref>]. A survey of the GF-IMU literature and its research areas can be found in [<xref ref-type="bibr" rid="b5-sensors-12-05310">5</xref>].</p>
<p>The rest of this paper is organized as follows: Section 2 gives a background about the angular motion estimation in a GF-IMU and describes the configuration used in this work. Section 3 lists the dynamic models which can be used for the Kalman filter process update. Section 4 gives a sensor error model with a review of the calibration procedure. Section 5 presents an extended Kalman filter (EKF) solution using a Singer model with appended bias parameters. Section 6 presents the observability analysis for the augmented state space model. Section 7 presents an EKF solution without appending bias parameters. Section 8 gives simulation results for the augmented model and Section 9 gives simulation results for the reduced model. Finally, Section 10 presents our conclusions.</p></sec>
<sec>
<label>2.</label>
<title>Angular Motion Estimation in a GF-IMU</title>
<p>Using certain fixed GF-IMU configurations of accelerometers, we get an angular acceleration vector and quadratic terms of the angular velocity. Quadratic angular velocity terms do not have an accumulative error as in the case when the angular acceleration is integrated. Proper filter setup combining the angular acceleration and the quadratic terms can assist in the convergence to the right sign as the quadratic terms have undetermined sign solutions. The integration of the different types of data coming from the GF-IMU has been a subject of intensive research. In [<xref ref-type="bibr" rid="b5-sensors-12-05310">5</xref>], an EKF solution using direct three state models based on Euler first order integration is given. The advantage of using such a model is that no assumption needs to be made about the dynamics of the motion and hence such a solution fits most scenarios. Reference [<xref ref-type="bibr" rid="b6-sensors-12-05310">6</xref>] gives a nine state model which includes the angular acceleration, the angular velocity and the translational acceleration vectors. The solution for that model uses an unscented Kalman filter (UKF) with constant angular acceleration and translational acceleration models and without appending the bias parameters. An EKF solution with measurement vector which uses three quadratic velocity terms measurements is given in [<xref ref-type="bibr" rid="b7-sensors-12-05310">7</xref>] for a configuration of nine fixed accelerometers. However, that GF-IMU configuration produces three quadratic terms besides the angular acceleration vector. We use dynamic models to describe the evolution of the state space in time to have an estimate of the angular motion. Bias parameters can be appended to the state vector if their estimation is desired. Though the first order Euler integration model has a simple form, some of the bias parameters, which exist in most of the inertial sensors, will not be observable. Hence, it fits cases where white noise is corrupting accelerometers measurements. The importance of this research is that within a proper dynamic model, the drifting biases can be estimated and hence the quality of the estimated angular velocity is improved greatly. Moreover, any prior information about the motion can be applied in the model to get an improved performance of angular motion estimation.</p>
<sec>
<label>2.1.</label>
<title>GF-IMU Fixed Accelerometers Configurations</title>
<p>The configuration shown in <xref ref-type="fig" rid="f1-sensors-12-05310">Figure 1</xref>, has four rigidly accelerometer triads, symbolized as A, B, C and D. We focus on configurations consisting of twelve mono-axial accelerometers that follow the rules listed by Zappa [<xref ref-type="bibr" rid="b8-sensors-12-05310">8</xref>].</p>
<p>Mainly, we consider this configuration because a minimum of twelve accelerometers are needed to determine angular velocity magnitude and direction (algebraic sign cannot be determined uniquely). The most amount of the angular motion information, which is the AIV composed of the nine angular terms shown in <xref rid="FD1" ref-type="disp-formula">Equations (1)</xref>–<xref rid="FD3" ref-type="disp-formula">(3)</xref> can be extracted from this configuration. For more information about this special GF-IMU, we refer the reader to [<xref ref-type="bibr" rid="b5-sensors-12-05310">5</xref>].</p>
<p>
<disp-formula id="FD1">
<label>(1)</label>
<mml:math id="mm1" display="block">
<mml:semantics id="sm1">
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>x</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>y</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>z</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula>
<disp-formula id="FD2">
<label>(2)</label>
<mml:math id="mm2" display="block">
<mml:semantics id="sm2">
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>x</mml:mi></mml:msub>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>y</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>x</mml:mi></mml:msub>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>y</mml:mi></mml:msub>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula>
<disp-formula id="FD3">
<label>(3)</label>
<mml:math id="mm3" display="block">
<mml:semantics id="sm3">
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>ω</mml:mi>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>ω</mml:mi>
<mml:mi>y</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>D</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>z</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>B</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>x</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>C</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mi>a</mml:mi>
<mml:mi>y</mml:mi>
<mml:mi>A</mml:mi></mml:msubsup></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>d</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec></sec>
<sec>
<label>3.</label>
<title>Dynamic Models to Be Considered</title>
<p>In general, a good model is important to extract the maximum amount of information from the observation. We will utilize the proper dynamic models in the angular motion estimation in the GF-IMU. We focus on the dynamic models used for maneuvering target tracking surveyed in [<xref ref-type="bibr" rid="b9-sensors-12-05310">9</xref>]. Tracking theory dynamic models are used previously to model the angular acceleration evolution in time [<xref ref-type="bibr" rid="b10-sensors-12-05310">10</xref>,<xref ref-type="bibr" rid="b11-sensors-12-05310">11</xref>]. This work can be considered as a spatial motion extension to the planar motion case given in [<xref ref-type="bibr" rid="b11-sensors-12-05310">11</xref>]. Assuming a certain type of motion the dynamic model can be formulated based on that assumption. The constant angular velocity model makes no use of the measured angular acceleration measurements so such a model is not a candidate for consideration in our work. Hence, we consider models in which the angular acceleration of the target is the descriptor of a target maneuver and modeled as a random process. Next, we describe the dynamic models that can be used for the Kalman filter process update.</p>
<sec>
<label>3.1.</label>
<title>Wiener-Process Angular Acceleration Model</title>
<p>This model assumes that the angular acceleration is a Wiener process, or more generally and precisely, the angular acceleration is a process with independent increments, which is not necessarily a Wiener process. This model is referred as a constant angular acceleration model (CAA) or a nearly constant angular acceleration model. It can be considered as a special case of a Gauss-Markov process. This model makes the angular acceleration a process with an increasing variance:
<disp-formula id="FD4">
<label>(4)</label>
<mml:math id="mm4" display="block">
<mml:semantics id="sm4">
<mml:mrow>
<mml:mover accent="true">
<mml:mi>α</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mo stretchy="false">(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:mi>w</mml:mi>
<mml:mo stretchy="false">(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The discrete-time form is given as:
<disp-formula id="FD5">
<label>(5)</label>
<mml:math id="mm5" display="block">
<mml:semantics id="sm5">
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>w</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>Since we have time uncorrelated noise, the corresponding state space representation of the Wiener sequence of angular acceleration vector combined with the angular velocity vector is given as:
<disp-formula id="FD6">
<label>(6)</label>
<mml:math id="mm6" display="block">
<mml:semantics id="sm6">
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mspace width="0.2em"/>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>+</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec>
<sec>
<label>3.2.</label>
<title>First-Order Markov Model (Singer Angular Acceleration Model)</title>
<p>This model was initially used for modeling linear acceleration [<xref ref-type="bibr" rid="b12-sensors-12-05310">12</xref>] and was lately used for angular acceleration modeling, as given in [<xref ref-type="bibr" rid="b10-sensors-12-05310">10</xref>]. It has much wider coverage than constant angular velocity or constant angular acceleration models. The Singer model can be adjusted using the specifications of the accelerometers used. The Singer model assumes that the target acceleration is a zero-mean stationary first-order Markov process. The time evolution of the angular acceleration in continuous time is written as:
<disp-formula id="FD7">
<label>(7)</label>
<mml:math id="mm7" display="block">
<mml:semantics id="sm7">
<mml:mrow>
<mml:mover accent="true">
<mml:mi>α</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mo>=</mml:mo>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi>α</mml:mi>
<mml:mo>+</mml:mo>
<mml:mi>w</mml:mi></mml:mrow></mml:semantics></mml:math></disp-formula>where <italic>w</italic> is a zero-mean white noise and <italic>β</italic> is the reciprocal of the time constant (or reciprocal of correlation time). The discrete form of this process is given as:
<disp-formula id="FD8">
<label>(8)</label>
<mml:math id="mm8" display="block">
<mml:semantics id="sm8">
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mi>e</mml:mi>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msup>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>σ</mml:mi>
<mml:mi>α</mml:mi></mml:msub>
<mml:msqrt>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:msup>
<mml:mi>e</mml:mi>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>2</mml:mn>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msup></mml:mrow></mml:msqrt>
<mml:msub>
<mml:mi>u</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula>where <italic>u<sub>i</sub></italic> is a sample generated from a Gaussian random number with a unit variance and <italic>σ<sub>a</sub></italic> is the steady-state variance. Since the correlation time 1/<italic>β</italic> is much larger than sampling time Δ<italic>t</italic>, the following approximation is used:
<disp-formula id="FD9">
<label>(9)</label>
<mml:math id="mm9" display="block">
<mml:semantics id="sm9">
<mml:mrow>
<mml:msup>
<mml:mi>e</mml:mi>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msup>
<mml:mo>≈</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>Considering a first-order linearization for the exponential term in <xref rid="FD9" ref-type="disp-formula">Equations (9)</xref>, the process can be approximated as:
<disp-formula id="FD10">
<label>(10)</label>
<mml:math id="mm10" display="block">
<mml:semantics id="sm10">
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mo stretchy="false">(</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:msqrt>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msqrt>
<mml:msub>
<mml:mi>σ</mml:mi>
<mml:mi>α</mml:mi></mml:msub>
<mml:msub>
<mml:mi>u</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>^</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The autocorrelation function Ψ<italic><sub>α</sub></italic> is exponentially decaying and given as:
<disp-formula id="FD11">
<label>(11)</label>
<mml:math id="mm11" display="block">
<mml:semantics id="sm11">
<mml:mrow>
<mml:msub>
<mml:mi mathvariant="normal">Ψ</mml:mi>
<mml:mi>α</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mi>E</mml:mi>
<mml:mo>{</mml:mo>
<mml:mi>α</mml:mi>
<mml:mo stretchy="false">(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo>+</mml:mo>
<mml:mi>τ</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:mi>α</mml:mi>
<mml:mo stretchy="false">(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>}</mml:mo>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mi>σ</mml:mi>
<mml:mi>α</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:msup>
<mml:mi>e</mml:mi>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mrow>
<mml:mo>|</mml:mo>
<mml:mi>τ</mml:mi>
<mml:mo>|</mml:mo></mml:mrow></mml:mrow></mml:msup></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The corresponding state space representation of the Wiener sequence angular acceleration model in 3D motion including angular velocity vector is given as:
<disp-formula id="FD12">
<label>(12)</label>
<mml:math id="mm12" display="block">
<mml:semantics id="sm12">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo stretchy="false">(</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mspace width="0.2em"/>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>+</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:msqrt>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msqrt>
<mml:msub>
<mml:mi>σ</mml:mi>
<mml:mi>α</mml:mi></mml:msub>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>u</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The variance is selected according to the ternary-uniform mixture as suggested in [<xref ref-type="bibr" rid="b10-sensors-12-05310">10</xref>].</p></sec>
<sec>
<label>3.3.</label>
<title>Other Models to Be Considered</title>
<p>Angular jerk, which is the derivative of the angular acceleration, can be used in the same way as that of the angular acceleration based models. Using angular jerk based models increases the dimension of the state space vector which increases the computational load.</p></sec></sec>
<sec>
<label>4.</label>
<title>Sensor Error Model and GF-IMU Calibration</title>
<p>In this section, we give a simple error model of the accelerometer which considers the bias only. The section ends with a review of simple calibration procedure which fits the GF-IMU.</p>
<sec>
<label>4.1.</label>
<title>The Accelerometer Error Model</title>
<p>Each accelerometer measurement is assumed to be corrupted by a bias <italic>b<sub>a</sub></italic> error component and a continuous white noise error component <italic>w<sub>acc</sub></italic>. The white noise usually has the unit of g/✓Hz, where <italic>g</italic> is the gravity, or its equivalent derivatives. All accelerometers are assumed to have a common upper bound for the noise variance and bias instability. The discrete-time white noise depends on the square root of the sampling time. The accelerometer measurement is modeled as:
<disp-formula id="FD13">
<label>(13)</label>
<mml:math id="mm13" display="block">
<mml:semantics id="sm13">
<mml:mrow>
<mml:mover accent="true">
<mml:mi>a</mml:mi>
<mml:mo>˜</mml:mo></mml:mover>
<mml:mo>=</mml:mo>
<mml:mi>a</mml:mi>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mi>a</mml:mi></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>w</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">acc</mml:mtext></mml:mrow></mml:msub>
<mml:mrow>
<mml:mrow>
<mml:msqrt>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:msqrt></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The variance of the discrete-time noise component <italic>R<sub>disc.</sub></italic> of each measurement of the acceleration is:
<disp-formula id="FD14">
<label>(14)</label>
<mml:math id="mm14" display="block">
<mml:semantics id="sm14">
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">disc</mml:mtext>
<mml:mo>^</mml:mo></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:mi>E</mml:mi>
<mml:mo>{</mml:mo>
<mml:msubsup>
<mml:mi>w</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">acc</mml:mtext></mml:mrow>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>}</mml:mo></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:mrow>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">acc</mml:mtext></mml:mrow></mml:msub></mml:mrow>
<mml:mo stretchy="true">/</mml:mo>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi></mml:mrow></mml:mrow></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>Every drifting accelerometer bias has the unit of <italic>g</italic> or its equivalent derivatives and is modeled as a random process driven by white noise. The previously described Markov model is used often to model the bias or we can use the following simple model of random walk:
<disp-formula id="FD15">
<label>(15)</label>
<mml:math id="mm15" display="block">
<mml:semantics id="sm15">
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>b</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>a</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>w</mml:mi>
<mml:mrow>
<mml:mi>b</mml:mi>
<mml:mi>a</mml:mi></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>In discrete-time the random walk bias model is given as:
<disp-formula id="FD16">
<label>(16)</label>
<mml:math id="mm16" display="block">
<mml:semantics id="sm16">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mrow>
<mml:mi>a</mml:mi>
<mml:mi>k</mml:mi></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mrow>
<mml:mi>a</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mrow>
<mml:mi>b</mml:mi>
<mml:mi>a</mml:mi>
<mml:mo>,</mml:mo>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec>
<sec>
<label>4.2.</label>
<title>The Measured AIV</title>
<p>Using the accelerometer's error model shown in <xref rid="FD13" ref-type="disp-formula">Equation (13)</xref>, we rewrite the measured AIV with inherited accelerometers' errors based on <xref rid="FD1" ref-type="disp-formula">Equations (1)</xref>–<xref rid="FD3" ref-type="disp-formula">(3)</xref> as:
<disp-formula id="FD17">
<label>(17)</label>
<mml:math id="mm17" display="block">
<mml:semantics id="sm17">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˜</mml:mo></mml:mover>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>x</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>x</mml:mi></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>W</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mo>⋮</mml:mo></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msubsup>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˜</mml:mo></mml:mover>
<mml:mi>z</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>9</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>w</mml:mi>
<mml:mn>9</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula>where <italic>b</italic><sub>1</sub>…<italic>b</italic><sub>9</sub> represent the new bias parameters and <italic>w</italic><sub>1</sub>…<italic>w</italic><sub>9</sub> represent the noise errors.</p></sec>
<sec>
<label>4.3.</label>
<title>Calibration and Initial Bias Estimation in a GF-IMU</title>
<p>In our setup, we adjust the separation distance manually to be unique for the three distributed triads with common orientation for all triads. Every accelerometer triad needs to be calibrated for three types of errors which are misalignment, scale factor and bias errors. Examples for the accelerometer triad calibration procedures can be found in [<xref ref-type="bibr" rid="b13-sensors-12-05310">13</xref>,<xref ref-type="bibr" rid="b14-sensors-12-05310">14</xref>]. The scale factor and misalignment parameters are contained in a three-by-three matrix and in our example they are calibrated only one time since they vary little with temperature change. The adopted calibration procedure is described with detailed equations in [<xref ref-type="bibr" rid="b5-sensors-12-05310">5</xref>]. Any remaining bias parameters in the accelerometers results in a biased AIV. Once the IMU is detected in reset position, the AIV will be due to accelerometers' bias and hence its bias can be captured. Keeping the GF-IMU in a static position means all angular information terms should be almost zero because the quadratic terms due to Earth rotation are extremely small for small separation distance. Hence, we find the initial value for nine bias parameters in the AIV. In case of using the simple model without appending the bias parameters, the calibration process can compensate for the bias parameters in the AIV.</p></sec></sec>
<sec>
<label>5.</label>
<title>An EKF Solution Using the Singer Model with Appended Bias Parameters</title>
<p>Though we have twelve accelerometers and this means we have twelve unknown bias parameters, we are interested in estimating the resulting nine bias parameters <italic>b</italic><sub>1</sub>…<italic>b</italic><sub>9</sub> in the AIV given in <xref rid="FD17" ref-type="disp-formula">Equation (17)</xref>. The state vector is composed of the angular velocity vector, the angular acceleration vector and the nine bias parameters given as:
<disp-formula id="FD18">
<label>(18)</label>
<mml:math id="mm18" display="block">
<mml:semantics id="sm18">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msup>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msup>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>α</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow>
<mml:mi>T</mml:mi></mml:msubsup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>x</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>y</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>x</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>y</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>z</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>α</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>7</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>8</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mn>9</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<sec>
<label>4.4.</label>
<title>Initialization</title>
<p>The initial state vector can be set as:
<disp-formula id="FD19">
<label>(19)</label>
<mml:math id="mm19" display="block">
<mml:semantics id="sm19">
<mml:mrow>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:mi>E</mml:mi>
<mml:mo>{</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn></mml:msub>
<mml:mo>}</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The initial estimation error covariance is given as:
<disp-formula id="FD20">
<label>(20)</label>
<mml:math id="mm20" display="block">
<mml:semantics id="sm20">
<mml:mrow>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mn>0</mml:mn>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:mi>E</mml:mi>
<mml:mo>{</mml:mo>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn></mml:msub>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo stretchy="false">)</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn></mml:msub>
<mml:mo>−</mml:mo>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mn>0</mml:mn>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo stretchy="false">)</mml:mo></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>}</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec>
<sec>
<label>4.5.</label>
<title>Prediction</title>
<p>Based on the previously described motion dynamic <xref rid="FD13" ref-type="disp-formula">Equation (13)</xref> and the accelerometer bias <xref rid="FD16" ref-type="disp-formula">Equation (16)</xref>, we can write the discrete-time space model as:
<disp-formula id="FD21">
<label>(21)</label>
<mml:math id="mm21" display="block">
<mml:semantics id="sm21">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>α</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo stretchy="false">(</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>6</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mspace width="0.2em"/>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>α</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>b</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>6</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mspace width="0.2em"/>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>α</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mi>a</mml:mi></mml:msub></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:msub>
<mml:mi>b</mml:mi>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>F</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>G</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The process covariance is computed as:
<disp-formula id="FD22">
<label>(22)</label>
<mml:math id="mm22" display="block">
<mml:semantics id="sm22">
<mml:mrow>
<mml:msub>
<mml:mi>Q</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mi>G</mml:mi>
<mml:msub>
<mml:mi mathvariant="normal">∑</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msup>
<mml:mi>G</mml:mi>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:mi mathvariant="normal">∑</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mi>E</mml:mi>
<mml:mo>{</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>}</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The <italic>a priori</italic> estimation error covariance is updated as:
<disp-formula id="FD23">
<label>(23)</label>
<mml:math id="mm23" display="block">
<mml:semantics id="sm23">
<mml:mrow>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>F</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:msubsup>
<mml:mi>F</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>Q</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The <italic>a priori</italic> state estimate is predicted as:
<disp-formula id="FD24">
<label>(24)</label>
<mml:math id="mm24" display="block">
<mml:semantics id="sm24">
<mml:mrow>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>F</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow>
<mml:mo>+</mml:mo></mml:msubsup></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec>
<sec>
<label>4.6.</label>
<title>Measurement Update</title>
<p>The measurement vector is composed of the angular acceleration vector and six quadratic terms of angular velocity combined with the nine bias parameters is given as:
<disp-formula id="FD25">
<label>(25)</label>
<mml:math id="mm25" display="block">
<mml:semantics id="sm25">
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>k</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msubsup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>7</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>8</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>9</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>10</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>11</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>12</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>13</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>14</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>15</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>k</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>v</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The measurement Jacobian matrix is computed as:
<disp-formula id="FD26">
<label>(26)</label>
<mml:math id="mm26" display="block">
<mml:semantics id="sm26">
<mml:mrow>
<mml:msub>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mfrac>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>v</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo></mml:mrow>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mrow></mml:mfrac></mml:mrow>
<mml:mo stretchy="true">∣</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>x</mml:mi>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>6</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>H</mml:mi>
<mml:msup>
<mml:mi>H</mml:mi>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>6</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>x</mml:mi>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup></mml:mrow></mml:msub>
<mml:mo>,</mml:mo>
<mml:mi>H</mml:mi>
<mml:mi>H</mml:mi>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The measurement error covariance matrix is computed as:
<disp-formula id="FD27">
<label>(27)</label>
<mml:math id="mm27" display="block">
<mml:semantics id="sm27">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mi>a</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mi>M</mml:mi></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msup>
<mml:mi>M</mml:mi>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:msup>
<mml:mi>ω</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mfrac>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">disc</mml:mtext>
<mml:mo>^</mml:mo></mml:mrow></mml:msub></mml:mrow>
<mml:mrow>
<mml:msup>
<mml:mi>d</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:mfrac>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mi>α</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mfrac>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">disc</mml:mtext>
<mml:mo>^</mml:mo></mml:mrow></mml:msub></mml:mrow>
<mml:mrow>
<mml:msup>
<mml:mi>d</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:mfrac>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:mi>M</mml:mi>
<mml:mo>=</mml:mo>
<mml:mfrac>
<mml:mrow>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">disc</mml:mtext>
<mml:mo>^</mml:mo></mml:mrow></mml:msub></mml:mrow>
<mml:mrow>
<mml:msup>
<mml:mi>d</mml:mi>
<mml:mn>2</mml:mn></mml:msup></mml:mrow></mml:mfrac>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>4</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>1</mml:mn>
<mml:mo stretchy="true">/</mml:mo>
<mml:mn>2</mml:mn></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The Kalman gain is updated as documented in literature e.g., Simon [<xref ref-type="bibr" rid="b15-sensors-12-05310">15</xref>]:
<disp-formula id="FD28">
<label>(28)</label>
<mml:math id="mm28" display="block">
<mml:semantics id="sm28">
<mml:mrow>
<mml:msub>
<mml:mi>K</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:mo stretchy="false">(</mml:mo>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:msubsup>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo stretchy="false">)</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:msubsup>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>R</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow>
<mml:mrow>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msup></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The <italic>a posteriori</italic> state estimate is updated as:
<disp-formula id="FD29">
<label>(29)</label>
<mml:math id="mm29" display="block">
<mml:semantics id="sm29">
<mml:mrow>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>K</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>y</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>−</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:msubsup>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mn>0</mml:mn>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The <italic>a posteriori</italic> estimation error covariance can be updated as:
<disp-formula id="FD30">
<label>(30)</label>
<mml:math id="mm30" display="block">
<mml:semantics id="sm30">
<mml:mrow>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>+</mml:mo></mml:msubsup>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo>−</mml:mo>
<mml:msub>
<mml:mi>K</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:msubsup>
<mml:mi>P</mml:mi>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec></sec>
<sec sec-type="methods">
<label>5.</label>
<title>Observability Analysis</title>
<p>Using the dynamic model gives us the possibility to have all the bias parameters in the resulting angular terms observable under some conditions. In this section, we determine under which conditions is the state space observable. First, we remove the noise in this observability analysis which leaves us with the simpler homogeneous state-space system given in continuous-time as:
<disp-formula id="FD31">
<label>(31)</label>
<mml:math id="mm31" display="block">
<mml:semantics id="sm31">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:munder accentunder="true">
<mml:mi>f</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">(</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:mi>A</mml:mi>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mi>y</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">(</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:mi>A</mml:mi>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>9</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>12</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>12</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>12</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>9</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula>where <italic>A</italic> is based on the CAA model and <italic>h</italic>(<italic>x</italic>) is the measurement function as described previously. We follow the local observability test based on Lie derivatives [<xref ref-type="bibr" rid="b16-sensors-12-05310">16</xref>] in a similar way to its use in [<xref ref-type="bibr" rid="b11-sensors-12-05310">11</xref>] for the one-dimensional angular motion. We compute <italic>L</italic>, which denotes the set of all finite linear combinations of Lie derivatives of the measurement vector with respect to <italic>f</italic>(<italic>x</italic>) for various values of constant input. For the <italic>i</italic>th row scalar measurement <italic>h<sub>i</sub></italic> of the measurement vector, the Lie derivative of a scalar measurement is defined as:
<disp-formula id="FD32">
<label>(32)</label>
<mml:math id="mm32" display="block">
<mml:semantics id="sm32">
<mml:mrow>
<mml:msub>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:mfrac>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub></mml:mrow>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mrow></mml:mfrac>
<mml:mo>^</mml:mo>
<mml:munder accentunder="true">
<mml:mi>f</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">(</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The zero-order Lie derivative of the measurement is the measurement itself, <italic>i.e.</italic>:
<disp-formula id="FD33">
<label>(33)</label>
<mml:math id="mm33" display="block">
<mml:semantics id="sm33">
<mml:mrow>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mn>0</mml:mn></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>Higher order Lie derivatives are computed as:
<disp-formula id="FD34">
<label>(34)</label>
<mml:math id="mm34" display="block">
<mml:semantics id="sm34">
<mml:mrow>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mi>k</mml:mi></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi>
<mml:mi>k</mml:mi></mml:msubsup></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>For our model, which has <italic>n</italic> states, the higher order Lie derivatives starting from the third derivative are entirely zero vectors as shown next:
<disp-formula id="FD35">
<label>(35)</label>
<mml:math id="mm35" display="block">
<mml:semantics id="sm35">
<mml:mrow>
<mml:mi>h</mml:mi>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>7</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>8</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>9</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>10</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>11</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>12</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>13</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>14</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mrow>
<mml:mn>15</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mover accent="true">
<mml:mi>h</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:mover accent="true">
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>¨</mml:mo></mml:mover>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mover accent="true">
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>¨</mml:mo></mml:mover>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:mo>…</mml:mo>
<mml:mo>,</mml:mo>
<mml:msup>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>n</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msup>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The system is observable if the observability matrix <italic>O</italic>, which is defined next, has a rank equal to <italic>n</italic>:
<disp-formula id="FD36">
<label>(36)</label>
<mml:math id="mm36" display="block">
<mml:semantics id="sm36">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:mi>O</mml:mi>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>0</mml:mn></mml:msub>
<mml:mo>,</mml:mo>
<mml:mi>u</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>d</mml:mi>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mn>0</mml:mn></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mo>⋮</mml:mo></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>d</mml:mi>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mn>0</mml:mn></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>p</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mo>⋮</mml:mo></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>d</mml:mi>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mrow>
<mml:mi>n</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mo>⋮</mml:mo></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>d</mml:mi>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mrow>
<mml:mi>n</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>p</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:mi>d</mml:mi>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mi>k</mml:mi></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo>
<mml:mo>=</mml:mo>
<mml:mfrac>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:msubsup>
<mml:mi>L</mml:mi>
<mml:mi>f</mml:mi>
<mml:mi>k</mml:mi></mml:msubsup>
<mml:mo stretchy="false">(</mml:mo>
<mml:msub>
<mml:mi>h</mml:mi>
<mml:mi>i</mml:mi></mml:msub>
<mml:mo stretchy="false">)</mml:mo></mml:mrow>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mrow></mml:mfrac></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula>where <italic>p</italic> is the number of measurements. In our case, we have nine measurements and fifteen states so the dimension of the observability matrix is 135 × 15. After removing the zero rows, we get the following reduced observability matrix containing the independent rows:
<disp-formula id="FD37">
<label>(37)</label>
<mml:math id="mm37" display="block">
<mml:semantics id="sm37">
<mml:mrow>
<mml:mtable columnalign="left">
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mtext mathvariant="italic">red</mml:mtext>
<mml:mo>^</mml:mo></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>11</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>9</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>9</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>21</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>9</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>31</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>9</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>11</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>211</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>211</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr columnalign="left">
<mml:mtd columnalign="left">
<mml:mrow>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>21</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>,</mml:mo>
<mml:msub>
<mml:mi>O</mml:mi>
<mml:mrow>
<mml:mn>31</mml:mn></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>4</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>4</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>4</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow></mml:semantics></mml:math></disp-formula></p></sec>
<sec>
<label>6.</label>
<title>An EKF Solution Using the Singer Model without Appending Bias Parameters</title>
<p>Without appending bias parameters, the state vector is reduced to the angular velocity and the angular acceleration vectors and it is given as:
<disp-formula id="FD38">
<label>(38)</label>
<mml:math id="mm38" display="block">
<mml:semantics id="sm38">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msup>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msup>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>x</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>y</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>z</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>=</mml:mo>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>x</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>y</mml:mi></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mover accent="true">
<mml:mi>ω</mml:mi>
<mml:mo>˙</mml:mo></mml:mover>
<mml:mi>z</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The reduced state vector is clearly observable because the quadratic angular terms can solve for the angular velocity as shown in [<xref ref-type="bibr" rid="b8-sensors-12-05310">8</xref>] and the angular acceleration terms are directly measurable so observability analysis in this case is not necessary.</p>
<sec>
<label>6.1.</label>
<title>Initialization</title>
<p>The initial state vector and initial estimation error covariance are assigned in a similar way as given in <xref rid="FD19" ref-type="disp-formula">Equations (19)</xref> and <xref rid="FD20" ref-type="disp-formula">(20)</xref>.</p></sec>
<sec>
<label>6.2.</label>
<title>Prediction</title>
<p>Based on the previously described dynamic model we can write the discrete-time space model as:
<disp-formula id="FD39">
<label>(39)</label>
<mml:math id="mm39" display="block">
<mml:semantics id="sm39">
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mo stretchy="false">(</mml:mo>
<mml:mn>1</mml:mn>
<mml:mo>−</mml:mo>
<mml:mi>β</mml:mi>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:mo stretchy="false">)</mml:mo>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mspace width="0.2em"/>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>α</mml:mi>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:mo>+</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi mathvariant="normal">Δ</mml:mi>
<mml:mi>t</mml:mi>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>w</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo>
<mml:mn>1</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The <italic>a priori</italic> estimation error covariance and state estimate are predicted in a similar way to <xref rid="FD22" ref-type="disp-formula">Equations (22)</xref> and <xref rid="FD23" ref-type="disp-formula">(23)</xref>.</p></sec>
<sec>
<label>6.3.</label>
<title>Measurement Update</title>
<p>The AIV, which is composed of the angular acceleration vector and six quadratic terms of angular velocity, is given as:
<disp-formula id="FD40">
<label>(40)</label>
<mml:math id="mm40" display="block">
<mml:semantics id="sm40">
<mml:mrow>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>4</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>5</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>6</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msubsup>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn>
<mml:mn>2</mml:mn></mml:msubsup></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>k</mml:mi>
<mml:mi>T</mml:mi></mml:msubsup>
<mml:mo>+</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>v</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>The measurement Jacobian matrix is computed as:
<disp-formula id="FD41">
<label>(41)</label>
<mml:math id="mm41" display="block">
<mml:semantics id="sm41">
<mml:mrow>
<mml:msub>
<mml:mi>H</mml:mi>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mrow>
<mml:mfrac>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:msub>
<mml:munder accentunder="true">
<mml:mi>h</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mi>k</mml:mi></mml:msub>
<mml:mo stretchy="false">(</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo>,</mml:mo>
<mml:munder accentunder="true">
<mml:mi>v</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mo stretchy="false">)</mml:mo></mml:mrow>
<mml:mrow>
<mml:mo>∂</mml:mo>
<mml:munder accentunder="true">
<mml:mi>x</mml:mi>
<mml:mo>_</mml:mo></mml:munder></mml:mrow></mml:mfrac></mml:mrow>
<mml:mo>|</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>x</mml:mi>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup></mml:mrow></mml:msub>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>I</mml:mi>
<mml:mrow>
<mml:mn>3</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:mi>H</mml:mi>
<mml:msup>
<mml:mi>H</mml:mi>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mn>0</mml:mn>
<mml:mrow>
<mml:mn>6</mml:mn>
<mml:mo>×</mml:mo>
<mml:mn>3</mml:mn></mml:mrow></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mrow>
<mml:mi>x</mml:mi>
<mml:mo>=</mml:mo>
<mml:msubsup>
<mml:mover accent="true">
<mml:mi>x</mml:mi>
<mml:mo>^</mml:mo></mml:mover>
<mml:mi>k</mml:mi>
<mml:mo>−</mml:mo></mml:msubsup></mml:mrow></mml:msub>
<mml:mo>,</mml:mo>
<mml:mi>H</mml:mi>
<mml:mi>H</mml:mi>
<mml:mo>=</mml:mo>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr>
<mml:mtr>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>1</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>2</mml:mn></mml:msub></mml:mrow></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd>
<mml:mtd>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:msub>
<mml:mi>x</mml:mi>
<mml:mn>3</mml:mn></mml:msub></mml:mrow></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow></mml:semantics></mml:math></disp-formula>The Kalman gain, measurement error covariance matrix and the <italic>a posteriori</italic> state estimate are computed in a similar way to <xref rid="FD28" ref-type="disp-formula">Equations (28)</xref>–<xref rid="FD30" ref-type="disp-formula">(30)</xref>.</p></sec></sec>
<sec sec-type="results">
<label>7.</label>
<title>Simulation Results for the Augmented Model</title>
<p>In this section, we give simulation results for a sinusoidal trajectory which is considered often in literature [<xref ref-type="bibr" rid="b7-sensors-12-05310">7</xref>,<xref ref-type="bibr" rid="b10-sensors-12-05310">10</xref>,<xref ref-type="bibr" rid="b11-sensors-12-05310">11</xref>]. Moreover, such a trajectory satisfies the observability condition of a non-zero angular acceleration vector.</p>
<sec>
<label>7.1.</label>
<title>Trajectory Profile and Parameters Setting</title>
<p>This scenario of motion is a two-dimensional harmonic angular oscillation. Three-dimensional harmonic angular oscillation is considered as a coning motion, however, the GF-IMU system responds better in this case because of having all the AIV terms as non-zero, which increases observability. The mathematical description of the angular motion is given as:
<disp-formula id="FD42">
<label>(42)</label>
<mml:math id="mm42" display="block">
<mml:semantics id="sm42">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mi>ω</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo>)</mml:mo></mml:mrow>
<mml:mo>=</mml:mo>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>m</mml:mi></mml:msub>
<mml:mo>sin</mml:mo>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>π</mml:mi>
<mml:mi>f</mml:mi>
<mml:mi>t</mml:mi></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:semantics></mml:math></disp-formula>
<disp-formula id="FD43">
<label>(43)</label>
<mml:math id="mm43" display="block">
<mml:semantics id="sm43">
<mml:mrow>
<mml:munder accentunder="true">
<mml:mi>α</mml:mi>
<mml:mo>_</mml:mo></mml:munder>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mi>t</mml:mi>
<mml:mo>)</mml:mo></mml:mrow>
<mml:mo>=</mml:mo>
<mml:mn>2</mml:mn>
<mml:mi>π</mml:mi>
<mml:mi>f</mml:mi>
<mml:msub>
<mml:mi>ω</mml:mi>
<mml:mi>m</mml:mi></mml:msub>
<mml:mo>cos</mml:mo>
<mml:mrow>
<mml:mo>(</mml:mo>
<mml:mrow>
<mml:mn>2</mml:mn>
<mml:mi>π</mml:mi>
<mml:mi>f</mml:mi>
<mml:mi>t</mml:mi></mml:mrow>
<mml:mo>)</mml:mo></mml:mrow>
<mml:msup>
<mml:mrow>
<mml:mrow>
<mml:mo>[</mml:mo>
<mml:mrow>
<mml:mtable>
<mml:mtr>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>1</mml:mn></mml:mtd>
<mml:mtd>
<mml:mn>0</mml:mn></mml:mtd></mml:mtr></mml:mtable></mml:mrow>
<mml:mo>]</mml:mo></mml:mrow></mml:mrow>
<mml:mi>T</mml:mi></mml:msup></mml:mrow></mml:semantics></mml:math></disp-formula></p>
<p>For a practical value of the accelerometer's noise and bias levels, we consider the specifications of the accelerometers manufactured by Analog Devices. We want to have a portable IMU so we choose the separation distance to be 0.4 <italic>m</italic>. Duration time <italic>T</italic> of 20 s was enough to get stable results with sampling time of 0.01 s.</p>
<p>The state vector is initialized properly around the true state with initial state error of 5% of the true value. The bias values were selected randomly from a distribution with one standard deviation of 2400 μg as shown in <xref ref-type="table" rid="t1-sensors-12-05310">Table 1</xref>. <xref ref-type="fig" rid="f2-sensors-12-05310">Figures 2</xref> and <xref ref-type="fig" rid="f3-sensors-12-05310">3</xref> show plots of the angular velocity and the angular acceleration profiles respectively.</p></sec>
<sec sec-type="methods|results">
<label>7.2.</label>
<title>Results and Analysis</title>
<p>Errors in the estimated angular velocity and angular acceleration vectors are plotted in <xref ref-type="fig" rid="f4-sensors-12-05310">Figures 4</xref> and <xref ref-type="fig" rid="f5-sensors-12-05310">5</xref>, respectively.</p>
<p>Clearly, we see a good convergence to the true angular velocity and angular acceleration components. However, there is a small oscillation in the <italic>x</italic> and y components of the angular velocity and angular acceleration. This is because we model the angular acceleration as a random process driven by white noise, while the true trajectory is a sinusoidal one. From simulations, the magnitude of this swing increases as the frequency of oscillation of the trajectory increases and <italic>vice versa</italic> if the trajectory's frequency of oscillation decreases. Therefore, the CAA and the Markov models are not suitable for highly dynamic oscillations. The plots of estimation errors in the nine bias parameters are shown in <xref ref-type="fig" rid="f6-sensors-12-05310">Figure 6</xref>.</p>
<p>The plots show the convergence of all estimated bias parameters in AIV to their exact value with small steady state error. Moreover, from extensive simulations we find that reducing the noise error level of accelerometers gives a smoother and a faster convergence of bias parameters.</p></sec>
<sec>
<label>7.3.</label>
<title>Effect of Improper Initialization</title>
<p>It is well known that proper initialization of the state vector for the EKF, which implies linearization, is important to avoid filter divergence. However, the filter can tolerate a limited level of initialization error if the nonlinearity is not high. Since there is no <italic>a priori</italic> information available about bias parameters in AIV from calibration, we initialize the bias vector with zeros. Errors in the estimated angular velocity and angular acceleration vectors for the improper initialization case are plotted in <xref ref-type="fig" rid="f7-sensors-12-05310">Figures 7</xref> and <xref ref-type="fig" rid="f8-sensors-12-05310">8</xref> respectively. Simulations show that the filter converges without problems but it takes longer time under the same values of bias stability which was used before.</p></sec></sec>
<sec sec-type="results">
<label>8.</label>
<title>Simulation Results for Non-Augmented Model</title>
<p>In this part of the simulations, we consider the same trajectory described previously, but with bias parameters non-appended to the state vector which means we simply ignored them. For this reduced model, we can find the criteria for ignoring bias parameters based on accelerometers specifications of bias and noise errors given in <xref ref-type="table" rid="t2-sensors-12-05310">Table 2</xref>.</p>
<p>At relatively high sampling rates (e.g., 0.01 s or more), the magnitude of the error due to white noise is about 10 times the magnitude of error due to remaining bias for a tactical grade accelerometer. Consequently, for this sampling rate, the noise error dominates the bias error in this accelerometer category and for this scenario the EKF model works without a big difference from the one without bias. Hence, ignoring the bias and approximating the error as white noise error model can be justified considering that the Kalman filter will tolerate such a small remaining bias error.</p>
<sec>
<label>8.1.</label>
<title>Trajectory Profile and Parameter Setting</title>
<p>We repeat the previously used trajectory profile with the same settings except for the noise and bias levels which are set to <italic>w<sub>acc.</sub></italic> = 100 μg/✓Hz and the bias selected randomly from distribution with one standard deviation of 100 <italic>μg</italic>. The chosen accelerometer specifications satisfy the criteria that the error is dominated by white noise error at the selected sampling rate of 0.01 s.</p></sec>
<sec sec-type="methods|results">
<label>8.2.</label>
<title>Results and Analysis</title>
<p>First, the execution time for this model was much smaller than that of the previous model, which has bias parameters appended, because we have a much simpler state model. Using a reduced model implies reducing computational load remarkably. The plots of errors in the estimated angular velocity and the estimated angular accelerations are shown in <xref ref-type="fig" rid="f9-sensors-12-05310">Figures 9</xref> and <xref ref-type="fig" rid="f10-sensors-12-05310">10</xref>, respectively. From both figures, we see a fast convergence to the true profile of angular velocity and angular acceleration. Again, we see a small oscillation in the <italic>x</italic> and <italic>y</italic> components of the angular velocity and the angular acceleration, which was explained previously in Subsection 8.2.</p>
<p>To see the effect of ignoring higher bias values in the AIV, we repeated the simulation with the same parameter values as used in subsection 8.1 and created a plot of the estimated angular velocity vector. Such values of parameters do not meet the criteria that accelerometer's error is dominated by white noise error and hence this results in a biased estimate of the angular velocity vector as shown in <xref ref-type="fig" rid="f11-sensors-12-05310">Figure 11</xref>.</p></sec></sec>
<sec sec-type="conclusions">
<label>9.</label>
<title>Conclusions</title>
<p>We have presented a novel solution for estimating the spatial angular motion and bias parameters in a GF-IMU utilizing the dynamic models. The integration scheme is performed using an EKF. Observability analysis for the augmented model shows that the state space model is observable whenever the angular acceleration vector has non-zero magnitude. Simulation results shows that the filter can estimate the angular motion and bias parameters in the AIV for proper and improper initialization. Moreover in case of using tactical grade accelerometers or better, the error is dominated by noise error and hence the model can possibly be reduced to include only angular motion terms without degrading performance. Further research can be done to estimate the remaining bias parameters in the accelerometers.</p></sec></body>
<back>
<ack>
<p>We greatly appreciate the support of the German Academic Exchange Service (DAAD) for the doctoral work of Ezzaldeen Edwan within the International Postgraduate Programme (IPP) Multi Sensorics. The German Research Foundation (DFG) has funded part of the work reported herein; grant number KN 876/2-1, which is gratefully acknowledged.</p></ack>
<ref-list>
<title>References</title>
<ref id="b1-sensors-12-05310"><label>1.</label><citation citation-type="web"><person-group person-group-type="author"><name><surname>El-Sheimy</surname><given-names>N.</given-names></name><name><surname>Niu</surname><given-names>X.</given-names></name></person-group><source>The Promise of MEMs to the Navigation Community</source><comment>Available online: <ext-link xlink:href="http://www.insidegnss.com/auto/IG0307el-sheimyFinal.pdf" ext-link-type="uri">http://www.insidegnss.com/auto/IG0307el-sheimyFinal.pdf</ext-link> (accessed on 27 February 2012)</comment></citation></ref>
<ref id="b2-sensors-12-05310"><label>2.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Sukkarieh</surname><given-names>S.</given-names></name><name><surname>Nebot</surname><given-names>E.M.</given-names></name><name><surname>Durrant-Whyte</surname><given-names>H.F.</given-names></name></person-group><article-title>A high integrity imu/gps navigation loop for autonomous land vehicle applications</article-title><source>IEEE Trans. Robot. Autom.</source><year>1999</year><volume>15</volume><fpage>572</fpage><lpage>578</lpage><pub-id pub-id-type="doi">10.1109/70.768189</pub-id></citation></ref>
<ref id="b3-sensors-12-05310"><label>3.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Chin-Woo</surname><given-names>T.</given-names></name><name><surname>Sungsu</surname><given-names>P.</given-names></name></person-group><article-title>Design of accelerometer-based inertial navigation systems</article-title><source>IEEE Trans. Instrum. Meas.</source><year>2005</year><volume>54</volume><fpage>2520</fpage><lpage>2530</lpage><pub-id pub-id-type="doi">10.1109/TIM.2005.858129</pub-id></citation></ref>
<ref id="b4-sensors-12-05310"><label>4.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Cardou</surname><given-names>P.</given-names></name><name><surname>Fournier</surname><given-names>G.</given-names></name><name><surname>Gagnon</surname><given-names>P.</given-names></name></person-group><article-title>A nonlinear program for angular-velocity estimation from centripetal-acceleration measurements</article-title><source>IEEE/ASME Trans. Mechatron</source><year>2011</year><volume>16</volume><fpage>932</fpage><lpage>944</lpage><pub-id pub-id-type="doi">10.1109/TMECH.2010.2057440</pub-id></citation></ref>
<ref id="b5-sensors-12-05310"><label>5.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Edwan</surname><given-names>E.</given-names></name><name><surname>Knedlik</surname><given-names>S.</given-names></name><name><surname>Loffeld</surname><given-names>O.</given-names></name></person-group><article-title>Constrained angular motion estimation in a gyro free IMU</article-title><source>IEEE Trans. Aerosp. Electron. Syst.</source><year>2011</year><volume>47</volume><fpage>596</fpage><lpage>610</lpage><pub-id pub-id-type="doi">10.1109/TAES.2011.5705694</pub-id></citation></ref>
<ref id="b6-sensors-12-05310"><label>6.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Schopp</surname><given-names>P.</given-names></name><name><surname>Klingbeil</surname><given-names>L.</given-names></name><name><surname>Peters</surname><given-names>C.</given-names></name><name><surname>Manoli</surname><given-names>Y.</given-names></name></person-group><article-title>Design, geometry evaluation, and calibration of a gyroscope-free inertial measurement unit</article-title><source>Sens. Actuat. A: Phys.</source><year>2010</year><volume>162</volume><fpage>379</fpage><lpage>387</lpage><pub-id pub-id-type="doi">10.1016/j.sna.2010.01.019</pub-id></citation></ref>
<ref id="b7-sensors-12-05310"><label>7.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Park</surname><given-names>S.</given-names></name><name><surname>Tan</surname><given-names>C.-W.</given-names></name><name><surname>Park</surname><given-names>J.</given-names></name></person-group><article-title>A scheme for improving the performance of a gyroscope-free inertial measurement unit</article-title><source>Sens. Actuat. A: Phys.</source><year>2005</year><volume>121</volume><fpage>410</fpage><lpage>420</lpage><pub-id pub-id-type="doi">10.1016/j.sna.2005.03.060</pub-id></citation></ref>
<ref id="b8-sensors-12-05310"><label>8.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Zappa</surname><given-names>B.</given-names></name><name><surname>Legnani</surname><given-names>G.</given-names></name><name><surname>van den Bogert</surname><given-names>A.J.</given-names></name><name><surname>Adamini</surname><given-names>R.</given-names></name></person-group><article-title>On the number and placement of accelerometers for angular velocity and acceleration determination</article-title><source>J. Dyn. Syst. Meas. Control</source><year>2001</year><volume>123</volume><fpage>552</fpage><lpage>554</lpage><pub-id pub-id-type="doi">10.1115/1.1386649</pub-id></citation></ref>
<ref id="b9-sensors-12-05310"><label>9.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Rong Li</surname><given-names>X.</given-names></name><name><surname>Jilkov</surname><given-names>V.P.</given-names></name></person-group><article-title>Survey of maneuvering target tracking. Part I. Dynamic models</article-title><source>IEEE Trans. Aerosp. Electron. Syst.</source><year>2003</year><volume>39</volume><fpage>1333</fpage><lpage>1364</lpage><pub-id pub-id-type="doi">10.1109/TAES.2003.1261132</pub-id></citation></ref>
<ref id="b10-sensors-12-05310"><label>10.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Oshman</surname><given-names>Y.</given-names></name><name><surname>Markley</surname><given-names>F.L.</given-names></name></person-group><article-title>Spacecraft attitude/rate estimation using vector-aided GPS observations</article-title><source>IEEE Trans. Aerosp. Electron. Syst.</source><year>1999</year><volume>35</volume><fpage>1019</fpage><lpage>1032</lpage><pub-id pub-id-type="doi">10.1109/7.784071</pub-id></citation></ref>
<ref id="b11-sensors-12-05310"><label>11.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Cardou</surname><given-names>P.</given-names></name><name><surname>Angeles</surname><given-names>J.</given-names></name></person-group><article-title>Estimating the angular velocity of a rigid body moving in the plane from tangential and centripetal acceleration measurements</article-title><source>Multibody Syst. Dyn.</source><year>2008</year><volume>19</volume><fpage>383</fpage><lpage>406</lpage><pub-id pub-id-type="doi">10.1007/s11044-007-9096-9</pub-id></citation></ref>
<ref id="b12-sensors-12-05310"><label>12.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Singer</surname><given-names>R.A.</given-names></name><name><surname>Behnke</surname><given-names>K.W.</given-names></name></person-group><article-title>Real-time tracking filter evaluation and selection for tactical applications</article-title><source>IEEE Trans. Aerosp. Electron. Syst.</source><year>1971</year><volume>AES-7</volume><fpage>100</fpage><lpage>110</lpage><pub-id pub-id-type="doi">10.1109/TAES.1971.310257</pub-id></citation></ref>
<ref id="b13-sensors-12-05310"><label>13.</label><citation citation-type="journal"><person-group person-group-type="author"><name><surname>Syed</surname><given-names>Z.F.</given-names></name><name><surname>Aggarwal</surname><given-names>P.</given-names></name><name><surname>Goodall</surname><given-names>C.</given-names></name><name><surname>Niu</surname><given-names>X.</given-names></name><name><surname>El-Sheimy</surname><given-names>N.</given-names></name></person-group><article-title>A new multi-position calibration method for mems inertial navigation systems</article-title><source>Meas. Sci. Technol.</source><year>2007</year><volume>18</volume><fpage>1897</fpage><lpage>1907</lpage><pub-id pub-id-type="doi">10.1088/0957-0233/18/7/016</pub-id></citation></ref>
<ref id="b14-sensors-12-05310"><label>14.</label><citation citation-type="confproc"><person-group person-group-type="author"><name><surname>Bragge</surname><given-names>T.</given-names></name><name><surname>Hakkarainen</surname><given-names>M.</given-names></name><name><surname>Tarvainen</surname><given-names>M.</given-names></name><name><surname>Liikavainio</surname><given-names>T.</given-names></name><name><surname>Arokoski</surname><given-names>J.</given-names></name><name><surname>Karjalainen</surname><given-names>P.</given-names></name></person-group><article-title>Calibration of triaxial accelerometer by determining sensitivity matrix and offsets simultaneously</article-title><conf-name>Proceedings of the 1st Joint ESMAC—GCMAS Meeting</conf-name><conf-loc>Amsterdam, The Netherlands</conf-loc><conf-date>September 2006</conf-date></citation></ref>
<ref id="b15-sensors-12-05310"><label>15.</label><citation citation-type="book"><person-group person-group-type="author"><name><surname>Simon</surname><given-names>D.</given-names></name></person-group><source>Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches</source><publisher-name>Wiley-Interscience</publisher-name><publisher-loc>Hoboken, NJ, USA</publisher-loc><year>2006</year></citation></ref>
<ref id="b16-sensors-12-05310"><label>16.</label><citation citation-type="book"><person-group person-group-type="author"><name><surname>Isidori</surname><given-names>A.</given-names></name></person-group><source>Nonlinear Control Systems</source><publisher-name>Springer</publisher-name><publisher-loc>Berlin, Germany</publisher-loc><year>1995</year></citation></ref></ref-list>
<sec sec-type="display-objects">
<title>Figures and Tables</title>
<fig id="f1-sensors-12-05310" position="float">
<label>Figure 1.</label>
<caption>
<p>A configuration of multiple distributed tri-axial accelerometers.</p></caption>
<graphic xlink:href="sensors-12-05310f1.gif"/></fig>
<fig id="f2-sensors-12-05310" position="float">
<label>Figure 2.</label>
<caption>
<p>Angular velocity trajectory profile.</p></caption>
<graphic xlink:href="sensors-12-05310f2.gif"/></fig>
<fig id="f3-sensors-12-05310" position="float">
<label>Figure 3.</label>
<caption>
<p>Angular acceleration trajectory profile.</p></caption>
<graphic xlink:href="sensors-12-05310f3.gif"/></fig>
<fig id="f4-sensors-12-05310" position="float">
<label>Figure 4.</label>
<caption>
<p>Angular velocity vector estimation errors.</p></caption>
<graphic xlink:href="sensors-12-05310f4.gif"/></fig>
<fig id="f5-sensors-12-05310" position="float">
<label>Figure 5.</label>
<caption>
<p>Angular acceleration vector estimation errors.</p></caption>
<graphic xlink:href="sensors-12-05310f5.gif"/></fig>
<fig id="f6-sensors-12-05310" position="float">
<label>Figure 6.</label>
<caption>
<p>Estimated and reference bias parameters in the AIV.</p></caption>
<graphic xlink:href="sensors-12-05310f6.gif"/></fig>
<fig id="f7-sensors-12-05310" position="float">
<label>Figure 7.</label>
<caption>
<p>Angular velocity vector estimation errors for improper initialization.</p></caption>
<graphic xlink:href="sensors-12-05310f7.gif"/></fig>
<fig id="f8-sensors-12-05310" position="float">
<label>Figure 8.</label>
<caption>
<p>Angular acceleration vector estimation errors for improper initialization.</p></caption>
<graphic xlink:href="sensors-12-05310f8.gif"/></fig>
<fig id="f9-sensors-12-05310" position="float">
<label>Figure 9.</label>
<caption>
<p>Angular velocity vector estimation errors for the reduced model.</p></caption>
<graphic xlink:href="sensors-12-05310f9.gif"/></fig>
<fig id="f10-sensors-12-05310" position="float">
<label>Figure 10.</label>
<caption>
<p>Angular acceleration vector estimation errors for the reduced model.</p></caption>
<graphic xlink:href="sensors-12-05310f10.gif"/></fig>
<fig id="f11-sensors-12-05310" position="float">
<label>Figure 11.</label>
<caption>
<p>Angular velocity vector estimation errors for the reduced model with large AIV bias values.</p></caption>
<graphic xlink:href="sensors-12-05310f11.gif"/></fig>
<table-wrap id="t1-sensors-12-05310" position="float">
<label>Table 1.</label>
<caption>
<p>Numerical values of the simulation parameters.</p></caption>
<table frame="hsides" rules="groups">
<thead>
<tr>
<th align="center" valign="top"><bold>Parameter (Unit)</bold></th>
<th align="center" valign="top"><bold><italic>ω<sub>m</sub></italic> (rad/s)</bold></th>
<th align="center" valign="top"><bold><italic>f</italic> (Hz)</bold></th>
<th align="center" valign="top"><bold><italic>w<sub>acc</sub></italic> (g/✓Hz)</bold></th>
<th align="center" valign="top"><bold><italic>bias</italic> (g)</bold></th>
<th align="center" valign="top"><bold><italic><sub>d</sub></italic> <sub>(m)</sub></bold></th></tr></thead>
<tbody>
<tr>
<td align="center" valign="top">Value</td>
<td align="center" valign="top">0.4112</td>
<td align="center" valign="top">0.5</td>
<td align="center" valign="top">200 <italic>μ</italic></td>
<td align="center" valign="top">2400 <italic>μ</italic></td>
<td align="center" valign="top">0.4</td></tr></tbody></table></table-wrap>
<table-wrap id="t2-sensors-12-05310" position="float">
<label>Table 2.</label>
<caption>
<p>Accelerometer Categories.</p></caption>
<table frame="hsides" rules="groups">
<thead>
<tr>
<th align="left" valign="top"><bold>Performance Parameter</bold></th>
<th align="center" valign="top"><bold>Consumer</bold></th>
<th align="center" valign="top"><bold>Automotive</bold></th>
<th align="center" valign="top"><bold>Tactical</bold></th>
<th align="center" valign="top"><bold>Navigation</bold></th></tr></thead>
<tbody>
<tr>
<td align="left" valign="top">Noise Floor VRW (μg/✓Hz)</td>
<td align="center" valign="top">2000</td>
<td align="center" valign="top">1000</td>
<td align="center" valign="top">100–400</td>
<td align="center" valign="top">5–10</td></tr>
<tr>
<td align="left" valign="top">Bias Stability (μg)</td>
<td align="center" valign="top">2400</td>
<td align="center" valign="top">1200</td>
<td align="center" valign="top">50–500</td>
<td align="center" valign="top">5–10</td></tr></tbody></table></table-wrap></sec></back></article>
