Precise Strapdown Inertial Navigation System
(PSINS)
Toolbox for Matlab
Version 1.1
02/06/2014
Gongmin Yan
Northwestern Polytechnical University, Xi’an, P.R.China
PSINS TOOLBOX G. YAN, NWPU, XI’AN, CHINA
Contents
1. Preface.............................................................................................................................................1
2. License.............................................................................................................................................1
3. System Requirements......................................................................................................................1
4. Quick Start.......................................................................................................................................2
5. Symbols & Conventions.................................................................................................................2
6. Library Functions............................................................................................................................3
6.1 Toolbox Initialization (psinsinit, glvs, glvf, psinsenvi)............................................................3
6.2 Askew Matrix & Cross Product (askew, iaskew, cros).............................................................3
6.3 Attitude Conversion (a2qua, q2mat, q2att, q2mat, m2att, m2qua, attsyn, yawcvt).................4
6.4 Rotation Vector (rv2q, rv2m, q2rv, m2rv, rotv, qupdt, qupdt2, mupdt)....................................4
6.5 Quaternion Operation (qconj, qmul, qmulv, lq2m, rq2m)........................................................5
6.6 Normalization (vnormlz, qnormlz, mnormlz)...........................................................................5
6.7 Platform Misalignment (qaddphi/qaddafa, qdelphi/qdelafa, qq2phi/ qq2afa).........................5
6.8 AVP & Pos Error Manipulation (avpset, avpseterr, avpadderr, avpcmp, posset, posseterr)....5
6.9 SIMU Manipulation (imuseterr, imuadderr, rfu, imuresample)...............................................5
6.10 Coning & Sculling (cnscl, conecoef, conedrift, sculldrift, conepolyn, scullpolyn,
conetwospeed).................................................................................................................................6
6.11 The Earth Related (earth, ethinit, ethupdate, p2cne, vn2dpos, pp2vn, la2dpos, xyz2blh,
blh2xyz)...........................................................................................................................................6
6.12 Attitude Determination By Vectors (sv2atti, dv2atti).............................................................6
6.13 Initial Alignment (alignsb, alignvn, alignfn, aligni0, alignWahba, aligncmps).....................6
6.14 SINS Alogrithm (insinit, insupdate, inspure, altfilt, etm, dsins)............................................7
6.15 Kalman Filter (kfinit, kffk, kfhk, kfc2d, kfupdate, kffeedback, kftypedef, RLS)..................7
6.16 Nonlinear Filter (ekf, ut, ukf, afamodel).................................................................................7
6.17 POS (POSProcessing, POSFusion, imugpssyn).....................................................................7
6.18 Display (timebar, resdisp).......................................................................................................7
6.19 Plot (imuplot, insplot, inserrplot, gpsplot, kfplot, POSplot, satplot, labeldef).......................8
6.20 Sensor Simulation (conesimu, scullsimu, imustatic, trjsegment, trjsimu, odsimu, bhsimu,
gpssimu)..........................................................................................................................................8
6.21 I/O File Manipulation (imufile, avpfile, trjfile, pos2gpx, binfile)..........................................8
6.22 Angle Unit Conversion (r2d, r2dm, r2dms, d2r, dm2r, dms2r)..............................................8
6.23 Some Mathematical Algorithms (avar, sumn, meann, cumint, rgkt4)....................................9
6.24 Markov Process Simulation (markov1, markov2)..................................................................9
6.25 Variable Manipulation (prealloc, setvals, nnts, varpack)........................................................9
6.26 GPS Toolkit (rinexReadN/O, findEph, satPosVel, rhoSatRec, lsPos, spp, etc.)....................9
6.27 Others (aa2mu, aa2phi, att2c, avpinterp, i0fvp).....................................................................9
7. Demo Examples............................................................................................................................10
7.1 Coning & Sculling Motion......................................................................................................10
7.2 Initial Alignment.....................................................................................................................10
7.3 Pure Inertial Navigation..........................................................................................................10
I
PSINS TOOLBOX G. YAN, NWPU, XI’AN, CHINA
7.4 SINS/GPS Integrated Navigation...........................................................................................10
7.5 DR & SINS/DR Integrated Navigation...................................................................................10
7.6 GPS Navigation.......................................................................................................................11
8. Version History..............................................................................................................................11
9. Contacts.........................................................................................................................................11
10. Algorithm Overview....................................................................................................................11
10.1 ‘Local-Level-North-Slaved’ SINS Differential Equations...................................................11
10.2 Discrete SINS Updating Algorithms.....................................................................................12
A) Attitude Updating.................................................................................................................12
B) Velocity Updating.................................................................................................................13
C) Position Updating.................................................................................................................14
10.3 SINS Linear Error Propagation Models................................................................................14
10.4 SINS Initial Align State-space Models on Pseudo-static Base.............................................16
A) Small misalignment angle KF Model..................................................................................16
B) Large Header misalignment angle EKF Model...................................................................17
C) Large misalignment angle UKF Model...............................................................................17
10.5 SINS/GPS Integrated Models...............................................................................................18
10.6 SINS/DR Integrated Models.................................................................................................19
A) DR Algorithm.......................................................................................................................19
B) DR Error Models..................................................................................................................20
C) SINS/DR Integrated State-space model...............................................................................20
10.7 Coning/Sculling Motion and the Error Compensation Algorithm.......................................21
A) Coning Algorithm................................................................................................................21
B) Sculling Algorithm...............................................................................................................22
10.8 Trajectory Profile & SIMU Sensor Simulation....................................................................23
A) Trajectory Profile Simulation...............................................................................................24
B) SIMU Sensor Simulation.....................................................................................................24
II
PSINS TOOLBOX G. YAN, NWPU, XI’AN, CHINA
1. Preface
Precise Strapdow Inertial Navigation System (PSINS) toolbox for Matlab is an open source
program package, primarily developed for inertial-grade or higher grade inertial navigation
system simulation and data processing. PSINS toolbox includes strapdown inertial sensor
(gyro & accelerometer) output simulation, initial self-alignment simulation, pure SINS
navigation algorithm simulation, SINS/DR & SINS/GPS integrated navigation simulation and
many other useful routes, all implemented by a bunch of powerful library functions. The
PSINS library functions are well modularized and organized, then they are easy to understand
and master. Surely, PSINS has the capability to processing real SIMU and GPS sampling data
with a little or even no modification. On the basis of this PSINS toolbox, users can quickly
and conveniently set up an inertial navigation solution to achieve their specific purpose.
2. License
The PSINS toolbox is distributed under the BSD 2-clause license
(see http://opensource.org/licenses/BSD-2-Clause). Users are permitted to download, copy,
modify and redistribute this toolbox freely as long as they comply with the following license.
Copyright (c) 2009-2014, Gongmin Yan, All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions
and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of
conditions and the following disclaimer in the documentation and/or other materials provided
with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3. System Requirements
When developing this toolbox, the author’s environment is:
1
PSINS TOOLBOX G. YAN, NWPU, XI’AN, CHINA
Microsoft Windows XP (SP3) + Matlab 7.9.0 (R2009b) + CPU 3.0GHz + RAM 4.0GB.
4. Quick Start
1) Copy the PSINS toolbox root folder ‘psins\’, including all subfolders and files, to
your computer.
2) Run ‘psins\psinsinit.m’ to initialize PSINS environment.
3) Run ‘psins\demos\test_SINS_trj.m’ to generate a moving trajectory.
4) Run ‘psins\demos\test_SINS_GPS.m’ to demonstrate SINS/GPS integrated
navigation.
5) There are many demo examples in ‘psins\demos’, such as coning & sculling motion
demonstration, initial alignment, pure inertial navigation and POS data fusion, etc.
6) Try to do some modification and put your exercise file under ‘psins\mytest’. Enjoy
yourself and may you find something helpful!
5. Symbols & Conventions
1) SINS: Strapdown Inertial Navigation System.
2) GPS: Global Positioning System.
3) DR: Dead Reckoning.
4) SIMU: Strapdown Inertial Measurement Unit.
5) POS: Positioning Orientation System.
6) i-frame: inertial frame.
7) e-frame: ECEF(Earth-Center-Earth-Fix) frame.
8) n-frame: navigation reference frame, with E-N-U (East-North-Up) pointing
orientations.
9) b-frame: carrier’s body frame (i.e. SIMU frame), with X-Right, Y-Forward and Z-Up
pointing orientations.
10) p-frame: computed navigation frame, misalignment angles from p-frame to n-frame
usually denoted as ‘phi’.
11) ts/fs: sampling interval / sampling frequency.
12) T or len: total simulation time or data length.
13) att/qnb/Cnb: att – Euler’s angles of body attitude, i.e. att = [pitch, roll, yaw], NOTE:
yaw follows the right-handed system convention ranging (-pi, pi]; qnb – quaternion;
Cnb – direct cosine matrix (DCM), the transformation matrix from n frame to b-
frame.
14) vn: body velocity, i.e. the n-frame linear velocity w.r.t e-frame.
15) pos: body geographic coordinates, pos = [lat, lon, hgt], where lat – latitude, lon –
longitude, hgt – height above sea level.
16) avp/avp0: avp = [att, vn, pos, t], t – time tag. Usually, avp0 specifies the initial
navigation parameters , i.e. avp0 = [att0, vn0, pos0].
17) eb/web/db/wdb: eb – gyro constant drift error; web – gyro angular random walk
coefficient; db – accelerometer constant bias; wdb – accelerometer velocity random
walk coefficient.
2
