The first group is called as PHI-model. This is nothing but the simple perturbation of the equations of kinematics (which are wrongfully called as INS equations among navigation engineers). Because the difference between the true attitude and the INS derived attitude is represented as the letter PHI, this model is called as PHI.

The second group is the PSI-model. In fact, this model is also obtained as a result of a certain perturbation. However, in the PSI-model we perturb the equation of kinematics around a fictitious navigation frame which is called as the “computer frame”. The difference between the INS derived attitude and computer frame is represented as the letter PSI. That is where this name comes from.

There are sufficient number of papers in the existing literature describing the derivation of psi-models. Especially the Benson’s short papers on the psi model (both his 1975 and 1978 dated papers) describe everything related with it in plain English. There are some other papers further unifying several concepts and deriving a bunch of other models also. However, Benson’s papers (especially the one titled “A Comparison of Two Approaches to Pure-Inertial and Doppler-Inertial Error Analysis”) is all you need to learn everything regarding PSI models.

Having learned both the PHI and PSI models, then you will face the real question: which model should you use? This is the main topic of this blog note.

The short answer is that you should prefer the PHI model in all your Kalman filter implementations.

This answer may at first be surprising for you as all the canonical sources about navigation systems usually favours the psi model. So, let me elaborate my answer a little bit more.

It is indeed true that PSI-model is more clean than the PHI model as the transport rate is not perturbed in it. Being clean means that less number of floating point calculations are required in the Kalman filter cycles. However, in today’s computing capabilities such a tinny reduction in the model computation is not important at all.

Even though psi-model does not have any clear advantage over phi-model, every navigation engineer must definitely learn how to derive PSI model even if he does not use it at all. (I personally had been able to learn the real meaning of navigation frame only after studying the PSI model.) Mostly because of its conceptual importance, navigation engineers learn it in the early stages of their careers and then continue to use it out of habitual tendency. This is in fact the main reason why psi-model is more commonly used in the navigation systems with high-grade sensors.

On the other hand, psi model has one big disadvantage that makes it not so suitable for low-grade units. One of the significant problems that we face in the design of low-cost systems is the azimuth initialization. High grade system can perform gyro-compasing to reduce the initial attitude uncertainty to the levels suitable for small-angle assumption. However, in low cost system, we almost always have to perform in-motion alignment starting with a large heading uncertainty. Because of the definition of the “computer frame”, the effect of large heading uncertainty manifest itself on the velocity errors in the PSI-models. Therefore both the position and the velocity errors are affected by the non-linearity of the large attitude errors in the PSI-models. On the other hand, the large heading uncertainty only affects the position errors in the PHI-models. Therefore, PHI-models behaves better than PSI-models under large azimuth errors.

As far as I know, in the entire literature it is only Scherzinger who uses a PSI-model based large heading filter. However, in his paper titled “Inertial Navigator Error Models For Large Heading Uncertainty”, he also recognizes the aforementioned difficulty of standard psi-models and therefore proposes a modifed version of it. I find his modified psi-model unnecessarily complex. I cannot see any advantage of his method over much easier (and almost standard) method described in “T. M. Pham, Kalman filter Mechanization for INS airstart, IEEE, 1992″.

As a result, if you are going to desing an INS with low-cost sensors, you should only consider using PHI-models as long as you do not have a robust mean of attitude initialization. You should always remember that under small angle assumption phi and psi models are equivalent. Therefore, there is absolutely no theoretical advantage of choosing one over the other. However, this does not mean that PSI-model is not important. On the contrary, if you are a navigation engineer you have to learn it by heart in order to understand the basic navigation frame concepts.

PS: See smoother_2filt_fwd and example_wander in the toolkit as introductory examples to the PSI-models. Also sys_llh_phipsi, sys_metric_phipsi and correctnav_Cen clearly shows the difference between phi and psi model implementations.

Theoretically, it is impossible to detect zero velocity instants by relying only on the accelerometers and gyroscopes. Even if we had perfect inertial sensors, which are completely noise free, and used the magnitude of the gravity and Earth rotation rate as our decision thresholds, our detection algorithm would not be able to differentiate a constant motion from a zero-velocity.

However, because of its importance especially for human motion tracking algorithms, we have to improvise some kind of a zero-motion detection method. As long as these algorithms do not generate too much false positives (zero-motion detection during a motion), we can still use them to diminish the accumulated velocity errors and, to a certain extent, to stabilize the roll/pitch errors.

In addition to this false-positive problem, zero-motion detectors also impose a big burden on the processing side. As I have described in my previous blog entries, we do not need inertial sensors to have very high data rates. As long as they provide angle and velocity increments, we can read their outputs at a very low rate without causing any algorithmic error. However, this is not the case for zero motion detectors at all. Zero-motion detectors need as many samples as possible to reduce their false positive probabilities. That is the reason why we need MEMS sensor producers to embed such decision algorithms into their sensors so that we don’t have to deal with all the problems associated with the high data rates.

Unfortunately, realizing this approach is not as easy as it may seem. First of all, what is a zero motion detection algorithm? Does anyone has ever answer this question? Or, does anyone have ever been able to come up with an optimal detector? The answer is unfortunately no. As a matter of fact the answer will always be “no”, because theoretically it is impossible to find one. All we can do is to adapt some ad-hoc method and hope for the best. In this case, what kind of algorithm can we expect from MEMS producers to embed in their system? Can such a thing be possible by any means?

Recently, I have been working on this issue. I tried to invent a method to be embedded in MEMS sensors such that it can be used for both ordinary and advanced users to detect zero motion instants. Ordinary users only need auto-decision result (regardless of its false positive rate), whereas an advanced user would possibly require all the raw sensor data to generate its own decision. A practical algorithm should also be capable of helping the user in between these groups by providing only the sufficient amount of useful data required for a meaningful decision.

At first I tried to formulate zero motion detection problem in terms of H-infinity settings. For a robust detection algorithm, we have to assume that nature always try to deceive us as much as possible. Therefore, I hoped I could come up with a meaningful 2-people zero-sum game definition and then robustly estimate the worst case motion. However, as in the case of my previous PhD topic, I totally failed. (I had studied the application of H-infinity theory to the navigation systems for 2 years before switching to my last PhD topic of redundant sensors and finally being able to graduate.)

Meanwhile, I realized one important fact about the motion: an acceptable motion definition certainly requires a scale to be associated to the data. Both a spike and almost a constant value in the sensor outputs may denote (hint) a motion. A spike cannot be detected if the data is analyzed at higher levels (averaged signals), whereas a small (almost) constant level output will be burried into additive sensor white noise if the analysis is performed at a very low level (row signal level).

As a result, I abandoned h-infinity concept and turned my attention into multi-resolution signal techniques. At the end, I came up with reasonable (non-orthogonal) signal bases each of which represent the change in different scales within a given data window. Together with these bases I also needed to derive some kind of threshold for the decision algorithm. I did not have to think about this part at all because I already knew that Allan variance coefficients is nothing but the Haar wavelet variance of the stationary signal. Therefore, previously computed Allan variance coefficients contain all the threshold values that I needed. Combining all these pieces of findings together, I finalized my zero-motion detector for low cost MEMS units. Given a data window, my detector first computes the signal coefficients for each basis function and then compares these coefficients with the threshold values to determine whether a motion exists or not.
In the following video, you can see the result of my zero motion detector. I wrote a simple application to broadcast the inertial sensor data of my “wannabe death” htc phone. I also coded simple python scripts to process these data in real time with my zero motion detector on the PC side. At the end, I output the result of the detector using a simple 2-color GUI. The red rectangle means a zero-motion detection instant and the green rectangle represents motion.

As you can see from the above video, my motion detection algorithm is at least capable of reasonably detecting motion/no motion instants. Although this video does not contain an example, my algorithm unavoidably suffers from false positives too. However, as the video proves, it can be still be considered sufficient for simple human motion tracking applications.
The most important feature of my algorithm is not its zero motion detection capability. The power of this algorithm comes from the fact that it relies on multi-resolution signal analysis. Therefore, it can serve for any user group equally well. If such an algorithm is embedded in an inertial sensor, the user can easily determine the amount of useful decision data to be delivered from the sensor. For instance, an average user can request the sensor to send only the biggest 5 coefficients, whereas an advanced user can request all the coefficients to reconstruct the original signal. In the above video, the decision is based on only the biggest coefficient which is all an ordinary user would probably care about.

With this final invention of mine, I am one step closer to my ultimate smart front-end design for MEMS inertial sensors. I know one day I will be able to convince some MEMS producers to listen to my genuine ideas.

“…because of the singularity problem of Euler angles, quaternion representation is used in this study…”

It looks quite a legitimate statement, doesn’t it. It is indeed true that there is a singularity problem with the Euler angles. However, in reality, this statement is nothing but a self-confession that the authors of the paper do not know the topic they intent to write a paper.
Let’s see why this is the case.

The orientation between two coordinate systems is always uniquely represented by an orthogonal matrix. We call it direction cosine matrix (dcm) in navigation related literature. However, mathematicians prefer to call these matrices as a member SO(3) (which means set of special orthogonal set. “special” denotes that we consider only orthogonal matrices with a determinant of +1).
The key fact about dcm is that it is a unique representation. There is one and only one dcm between 2 coordinate frames.
On the other hand, dcm is an over representation. Therefore, in navigation algorithms we may prefer to use other representations with less number of elements such as euler angles, rotation vectors and quaternions.
Euler angles and rotation vectors consist of only 3 elements. However, they are not unique. In other words, between any coordinate systems we can define at least 2 different sets of Euler angles and/or rotation vectors. Furthermore, there are certain orientations which can be represented by infinitely many different euler angles/rotation vectors (e.g. 0 degree rotation vectors or 90 degree pitch for RPY).
In other words, neither euler angles nor rotation vectors are unique. The transformation between 2 coordinate systems can be represented by at least one set of euler angle/rotation vector. This is what we mean by singularity problem. Singularity does NOT mean that euler angles/rotation vectors are not defined for certain orientations. We can always use them to represent any arbitrary orientation regardless of this so-called singularity problem.
Quaternions are nothing but 4-element representation of the rotation vectors. The addition of this one more element makes quaternions also unique like dcm. Therefore, given any 2 coordinate systems, we can define one and only one quaternion.
As a result, in your next paper, if you try to state some reason for your choice of the quaternions, you had better use the following sentence instead of the one above:

“…because of the singularity problem of the rotation vectors, quaternion representation is used in this study…”

At the end of the day, any experienced navigation system designer knows that selection of attitude mechanization is completely personal. I usually prefer to use different mechanization in different projects just to keep my memory refreshed. Therefore, it is best to avoid using rookie statements like the above ones completely.
Finally, If you decide to use quaternions, do not use 4 states to represents errors on them. Small angle attitude errors are always represented by 3 states. Doing otherwise is another self-confession of ignorance. I am planning to elaborate this topic in a later blog entry.

1. Sequential ADCs
2. Flash ADCs
3. Sigma-Delta ADCs (Over Sampled ADCs)

(For a complete review of ADC technology, I suggest everyone reading Analog Devices’ ADC handbook). Sequential and Flash ADCs are fast but have a low resolution. In order to sample a gyroscope with a resolution of 24bit or more, a sigma-delta ADC (ΣΔ ADC) has to be used.
ΣΔ converters internally sample the analog inputs using a 1 bit ADC with a very high rate (that is why they are called as over-sampled ADC). After 1bit sampling operation, these 1bit samples are processed with a digital filter (and downsampled) to generate high resolution samples of the analog input. The digital filter used in ΣΔ ADC has a low pass characteristic. Therefore, ΣΔ ADCs cannot be used for high BW signals.
High speed and high resolution ΣΔ ADCs with simultaneous multiple channels can be very expensive. (they can cost more than 200$). Therefore, IMU designers who use this traditional approach find themselves in a situation in which they have to make a compromise between speed and resolution.
Having summarized the traditional approach, I can start attacking and blaming everyone who blindly follows this approach.

The main point that these people fail to understand is that we never need acceleration and rotation rates to solve the INS equations. All we need is the integral of these quantities (a.k.a angle and velocity increments). In this case, why don’t we integrate the signal with an analog front-end and then sample only this integral value? Or, even better, why don’t we use a front-end which integrates and samples the rate signal at the same time so that we don’t bother ourselves with integrating the signal (either in digital or analog domain) at all?

Here is a block diagram of such a circuit that can be used to integrate and digitize analog signals. It is in fact nothing but the simplest form of an ΣΔ modulator. Despite its simplicity it accomplishes exactly what we need.

The first summation junction is used to add sensor outputs when more than 1 inertial sensor is used per axis (i.e. orthogonal redundant configurations). Redundancy is a life saver. I strongly recommend everyone to use it even if only 1 sensor per axis is used. The feedback loop is a sigma-delta modulator. The 1 bit ADC is essentially a comparator. The feedback loop stabilizes the integrator output at “0”. Without such a feedback loop, the capacitor would not operate in the linear region. The 1 bit DAC generates either Vmax or 0 (we assume that sensor outputs changes only in the range of [Vmax 0]). The clock of the D flip-flop determines the time support of each rectangular pulse generated by the 1bit DAC. When the output of the latch (D flip flop) is high, the counter increments. Thus, the counter output is equal to the integral of the feedback signal. Therefore, it approximates the integral of the input. Thanks to the feedback loop, the maximum error on this integral is equal to dt*Vmax where dt is the period of the clock. Therefore, the faster is the clock, the better is the approximation. Regardless of the total integration time the error is bounded by this value. (In rate sampling systems the integral error keeps increasing in time without a bound. That is why we need very high BW to reduce the integration errors in rate sampling structures). Even if a moderate clock is used (e.g. 1Mhz) this error becomes completely negligible with respect to any other error source. Therefore, this integral sampling method can safely be assumed errorless.

The following figure shows a very primitive circuit which realizes the above block diagram. It assumes that the analog signal varies between Vmax and “0”. An analog designer would probably laugh at the simplicity of this circuit (no op-amp bias current compensation, no voltage isolation etc). But still, I suppose it will work as intended.

As a result, the approach described above has 2 distinct advantages which make it superior to any ADC based approach:

1. The quantization errors on the angle and velocity increments computed with this circuit is negligible.
2. A rate sensor must be sampled very fast in order to minimize the errors on the integrals. The above circuit computes the integrals in the analog domain. You only need to read the counter output whenever you need the angle/velocity increment values. Therefore, it provides a great flexibility on the timing requirements.

Those of you who have some experience on the INSs may find this a little bit disturbing. Basically, both “C” and the “a” changes in an update interval. Therefore, such a first order approximation should not be theoretically valid.

Basically you are right. A decent INS implementation must consider this effect. In general, such integral compensations are performed as a part of the coning and sculling algorithms (which are now part of the toolkit). Although these algorithms are meant to remove the algorithmic bias effects generated by the so-called coning and sculling motions, the intra-minor steps of these algorithms also take care of the integral compensation terms. After compensated by these algorithms, the IMU outputs can be directly used in the INS without requiring any special integration considerations in the strapdown implementation.

The beautiful part of this approach is that the IMU and the INS algorithms can be distinctively separated. The coning and sculling algorithms are black-box type algorithms for IMU sections (or boards). They operate only on the sensor outputs in a user defined interval and provide compensated outputs which are later processed by the strapdown algorithms like the ones in the toolkit. That is why, despite the simple integration approximation, the strapdown implementations in the toolkit are theoretically correct.

Most MEMS producers are not aware of this very simple fact. They only provide inertial measurements (i.e. acceleration and rotation rate) at the highest possible sensor rate and force to INS designers to read all the data separately. But INS designers don’t need such high rates. Even the missiles cruising in the subsonic region executes the INS algoithms at 100-200Hz rates. Only the compensated senor outputs at a moderate rate is sufficient for most commercial applications. As explained above, thanks to the IMU compensation algorithms, no integral error is induced in the strapdown algorithms even with the fairly slow updates rates.

It will take probably 10 more years for MEMS producers to think of providing low-rate compensated sensor outputs rather than very high rate raw measurements. They are really too stupid to understand even the simplest things about inertial navigation. Meanwhile, it is us (the INS designers) who is going to suffer from their stupidity.

As an example consider the inertial sensors in the smart phones. If you want to design a navigation system based on these sensors you have to make the OS (i.e android) read data at least 200Hz. Just trying to read data at such high rates costs tremendous processing power for nothing. Furhtermore, at such high rates synchronization becomes impossible. (As a matter of fact the time tags in the sensor data for the android is completely useless either.) It will be more than sufficient to read inertial sensor outputs at rates lower than 25Hz (even much lower for most human tracking applications) if the sensors provide compensated outputs. Also in this case, the unknown time jitter problem would also be less severe (or perhaps completely removed).

I am really disgusted by the overall ignorance in the inertial sensor industry. They don’t know and they don’t try to learn.

Here is a video showing a sample output with some real data:

In this video, the embedded navigation code processed the sensor data at 100HZ. The Kalman filter covariance update was also set to 100Hz. The GPS outputs at 1HZ and some occasional heading information was processed by the 21 state Kalman filter to stabilize the INS outputs during the navigation.

As can be seen from the left-upper corner (the time tag information), the real duration of the sensor data used in this video was only 320 seconds. However, it took 15 minutes for the phone to process the data (this can be verified from the phone’s clock seen on the upper-right corner).

One reason for this huge execution duration was the screen capture application that was run together with the navigation code. When I ran the navigation code by itself without any screen capture the total execution duration dropped to 440 seconds which was again 120s longer than the real navigation duration.

In my desktop PC, it takes less than 10 seconds for the same navigation code to process the entire data. It is clear from these time results that the processors in the smartphones, which do not have a math-coprocessor, are not fast enough for real time navigation with the single speed INS implementations.

This simple experiment shows that a gps/ins system for the smartphones must have 3-speed structure with the following components:

1. High speed INS update (for inertial sensors)
2. Low speed INS update (for navigation frame updates)
3. Low speed KF covariance updates.

Furthermore, it would be quite wise if  “item 3″ is implemented as an additional thread.  However, in this case the asynchronous KF updates should be well synchronized with the covariance update thread in order to prevent disastrous mistakes.