Inertial Navigation Syatem:

An inertial navigation system is a navigation aid that uses a

computer and motion sensors to continuously track the position,
orientation, and velocity (direction and speed of movement) of a
vehicle without the need for external references. Other terms used to
refer to inertial navigation systems or closely related devices include
inertial guidance system, inertial reference platform, and many
other variations.

An inertial navigation system includes at least a computer and a

platform or module containing accelerometers, gyroscopes, or other
motion-sensing devices. The INS is initially provided with its position
and velocity from another source (a human operator, a GPS satellite
receiver, etc.), and thereafter computes its own updated position and
velocity by integrating information received from the motion sensors.
The advantage of an INS is that it requires no external references in
order to determine its position, orientation, or velocity once it has
been initialized.

An INS can detect a change in its geographic position (a move

east or north, for example), a change in its velocity (speed and
direction of movement), and a change in its orientation (rotation about
an axis). It does this by measuring the linear and angular
accelerations applied to the system. Since it requires no external
reference (after initialization), it is immune to jamming and deception.

Inertial-navigation systems are used in many different types of

vehicles, including aircraft, submarines, spacecraft, and guided
missiles. However, their cost and complexity does place constraints
on the environments in which they are practical for use.

Gyroscopes measure the angular velocity of the system in the

inertial reference frame. By using the original orientation of the
system in the inertial reference frame as the initial condition and
integrating the angular velocity, the system's current orientation is
known at all times. This can be thought of as the ability of a
blindfolded passenger in a car to feel the car turn left and right or tilt
up and down as the car ascends or descends hills. Based on this
information alone, he knows what direction the car is facing but not
how fast or slow it is moving, or whether it is sliding sideways.

Accelerometers measure the linear acceleration of the system

in the inertial reference frame, but in directions that can only be
measured relative to the moving system (since the accelerometers
are fixed to the system and rotate with the system, but are not aware
of their own orientation). This can be thought of as the ability of a
blindfolded passenger in a car to feel himself pressed back into his
seat as the vehicle accelerates forward or pulled forward as it slows
down; and feel himself pressed down into his seat as the vehicle
accelerates up a hill or rise up out of his seat as the car passes over
the crest of a hill and begins to descend. Based on this information
alone, he knows how the vehicle is moving relative to itself, that is,
whether it is going forward, backward, left, right, up (toward the car's
ceiling), or down (toward the car's floor) measured relative to the car,
but not the direction relative to the Earth, since he did not know what
direction the car was facing relative to the Earth when he felt the
accelerations.

The INS used here is made by Crossbow company with model number
NAV420CA.This is a combined navigation, attitude and heading reference
system that measures stabilized pitch, roll and yaw angles in a dynamic
environment along with GPS-based position and velocity.
Gyroscope:
A gyroscope is a device for measuring or maintaining
orientation, based on the principles of angular momentum. The
device is a spinning wheel or disk whose axle is free to take any
orientation. This orientation changes much less in response to a
given external torque than it would without the large angular
momentum associated with the gyroscope's high rate of spin. Since
external torque is minimized by mounting the device in gimbals, its
orientation remains nearly fixed, regardless of any motion of the
platform on which it is mounted.

Fig: Diagram of a gyro wheel. Reaction arrows about the output axis (blue)
correspond to forces applied about the input axis (green), and vice versa.

Within mechanical systems or devices, a conventional

gyroscope is a mechanism comprising a rotor journaled to spin about
one axis, the journals of the rotor being mounted in an inner gimbal or
ring, the inner gimbal being journaled for oscillation in an outer gimbal
which in turn is journaled for oscillation relative to a support. The
outer gimbal or ring is mounted so as to pivot about an axis in its own
plane determined by the support. The outer gimbal possesses one
degree of rotational freedom and its axis possesses none. The inner
gimbal is mounted in the outer gimbal so as to pivot about an axis in
its own plane, which axis is always perpendicular to the pivotal axis of
the outer gimbal.

The axle of the spinning wheel defines the spin axis. The inner
gimbal possesses two degrees of rotational freedom and its axis
possesses one. The rotor is journaled to spin about an axis which is
always perpendicular to the axis of the inner gimbal. So, the rotor
possesses three degrees of rotational freedom and its axis
possesses two. The wheel responds to a force applied about the
input axis by a reaction force about the output axis.

The behavior of a gyroscope can be most easily appreciated by

consideration of the front wheel of a bicycle. If the wheel is leaned
away from the vertical so that the top of the wheel moves to the left,
the forward rim of the wheel also turns to the left. In other words,
rotation on one axis of the turning wheel produces rotation of the third
axis.

A gyroscope flywheel will roll or resist about the output axis

depending upon whether the output gimbals are of a free- or fixed-
configuration. Examples of some free-output-gimbal devices would be
the attitude reference gyroscopes used to sense or measure the
pitch, roll and yaw attitude angles in a spacecraft or aircraft.

The center of gravity of the rotor can be in a fixed position. The

rotor simultaneously spins about one axis and is capable of oscillating
about the two other axes, and thus, except for its inherent resistance
due to rotor spin, it is free to turn in any direction about the fixed
point. Some gyroscopes have mechanical equivalents substituted for
one or more of the elements, e.g., the spinning rotor may be
suspended in a fluid, instead of being pivotally mounted in gimbals. A
control moment gyroscope (CMG) is an example of a fixed-output-
gimbal device that is used on spacecraft to hold or maintain a desired
attitude angle or pointing direction using the gyroscopic resistance
force.

In some special cases, the outer gimbal (or its equivalent) may
be omitted so that the rotor has only two degrees of freedom. In other
cases, the center of gravity of the rotor may be offset from the axis of
oscillation, and thus the center of gravity of the rotor and the center of
suspension of the rotor may not coincide.

Properties:

A gyroscope in operation with freedom in all three axes. The

rotor will maintain its spin axis direction regardless of the orientation
of the outer frame.

A gyroscope exhibits a number of behaviors including

precession and nutation. Gyroscopes can be used to construct
gyrocompasses which complement or replace magnetic compasses
(in ships, aircraft and spacecraft, vehicles in general), to assist in
stability (bicycle, Hubble Space Telescope, ships, vehicles in general)
or be used as part of an inertial guidance system. Gyroscopic effects
are used in toys like yo-yos and Powerballs. Many other rotating
devices, such as flywheels, behave gyroscopically although the
gyroscopic effect is not used.

The fundamental equation describing the behavior of the

gyroscope is:

where the vectors and are, respectively, the torque on the

gyroscope and its angular momentum, the scalar is its moment of
inertia, the vector is its angular velocity, and the vector is its
angular acceleration.

It follows from this that a torque applied perpendicular to the

axis of rotation, and therefore perpendicular to , results in a motion
perpendicular to both and . This motion is called precession. The
angular velocity of precession is given by the cross product:

Precession can be demonstrated by placing a spinning

gyroscope with its axis horizontal and supported loosely (frictionless
toward precession) at one end. Instead of falling, as might be
expected, the gyroscope appears to defy gravity by remaining with its
axis horizontal, when the other end of the axis is left unsupported and
the free end of the axis slowly describes a circle in a horizontal plane,
the resulting precession turning. This effect is explained by the above
equations. The torque on the gyroscope is supplied by a couple of
forces: gravity acting downwards on the device's centre of mass, and
an equal force acting upwards to support one end of the device. The
motion resulting from this torque is not downwards, as might be
intuitively expected, causing the device to fall, but perpendicular to
both the gravitational torque (downwards) and the axis of rotation
(outwards from the point of support), i.e. in a forward horizontal
direction, causing the device to rotate slowly about the supporting
point.

As the second equation shows, under a constant torque, the

gyroscope's speed of precession is inversely proportional to its
angular momentum. This means that, for instance, if friction causes
the gyroscope's spin to slow down, the rate of precession increases.
This continues until the device is unable to rotate fast enough to
support its own weight, when it stops precessing and falls off its
support, mostly because friction against precession cause another
precession that goes to cause the fall.

By convention, these three vectors, torque, spin, and

precession, are all oriented with respect to each other according to
the right-hand rule.

To easily ascertain the direction of gyro effect, simply remember

that a rolling wheel tends, when entering a corner, to turn over to the
inside.

Accelerometers:
 An accelerometer is a device for measuring acceleration and
gravity induced reaction forces. Single- and multi-axis models are
available to detect magnitude and direction of the acceleration as a
vector quantity. Accelerometers can be used to sense inclination,
vibration, and shock. They are increasingly present in portable
electronic devices.

An accelerometer measures the acceleration and gravity it

experiences. Both are typically expressed in SI units meters/second2
(m·s-2) or popularly in terms of g-force.

The effects of gravity and acceleration are indistinguishable,

following Einstein's equivalence principle. As a consequence, the
output of an accelerometer has an offset due to local gravity. This
means that, perhaps counter intuitively, an accelerometer at rest on
the earth's surface will actually indicate 1 g along the vertical axis. To
obtain the acceleration due to motion alone, this offset must be
subtracted. Along all horizontal directions, the device yields
acceleration directly. Conversely, the device's output will zero during
free fall, where the acceleration exactly follows gravity. This includes
use in an earth orbiting spaceship, but not a (non-free) fall with air
resistance, where drag forces reduce the acceleration until terminal
velocity is reached, at which point the device would once again
indicate the 1 g vertical offset.

For the practical purpose of finding the acceleration of objects

with respect to the earth, such as for use in an inertial navigation
system, the correction due to gravity along the vertical axis is usually
made automatically, e.g. by calibrating the device at rest.

Modern accelerometers are often small micro electro-

mechanical systems (MEMS), and are indeed the simplest MEMS
devices possible, consisting of little more than a cantilever beam with
a proof mass (also known as seismic mass) and some type of
deflection sensing circuitry. Under the influence of gravity or
acceleration the proof mass deflects from its neutral position. The
deflection is measured in an analog or digital manner. Another type of
MEMS-based accelerometer contains a small heater at the bottom of
a very small dome, which heats the air inside the dome to cause it to
rise. A thermocouple on the dome determines where the heated air
reaches the dome and the deflection off the center is a measure of
the acceleration applied to the sensor.

Single-axis, dual-axis, and triple-axis models exist to measure

acceleration as a vector quantity or just one or more of its
components. MEMS accelerometers are available in a wide variety of
measuring ranges, reaching up to thousands of g's.

Accelerometers can be used to measure vibration on cars,

machines, buildings, process control systems and safety installations.
They can also be used to measure seismic activity, inclination,
machine vibration, dynamic distance and speed with or without the
influence of gravity. Applications for accelerometers that measure
gravity, wherein an accelerometer is specifically configured for use in
gravimetry, are called gravimeters.

One of the most common uses for MEMS accelerometers is in

airbag deployment systems for modern automobiles. In this case the
accelerometers are used to detect the rapid negative acceleration of
the vehicle to determine when a collision has occurred and the
severity of the collision. The widespread use of accelerometers in the
automotive industry has pushed their cost down dramatically.
Magnetometer:
A magnetometer is a scientific instrument used to measure the
strength and/or direction of the magnetic field in the vicinity of the
instrument.

Earth's magnetism varies from place to place and differences in the

Earth's magnetic field (the magnetosphere) can be caused by two
things:

1. The differing nature of rocks

2. The interaction between charged particles from the sun and the
magnetosphere

Fig: magnetometer Circuit Diagram

Uses
 Magnetometers are used in geophysical surveys to find
deposits of iron because they can measure the magnetic field
variations caused by the deposits. Magnetometers are also used to
detect archaeological sites, shipwrecks and other buried or
submerged objects. Magnetic anomaly detectors detect submarines
for military purposes.

A magnetometer can also be used by satellites like GOES to

measure both the magnitude and direction of the earth's magnetic
field.

They are used in directional drilling for oil or gas to detect the
azimuth of the drilling tools near the drill bit. They are most often
paired up with accelerometers in drilling tools so the both the
inclination and azimuth of the drill bit can be found.

Magnetometers are very sensitive, and can give an indication of

possible auroral activity before one can see the light from the aurora.
A grid of magnetometers around the world constantly measures the
effect of the solar wind on the earth's magnetic field.

Types

Magnetometers can be divided into two basic types:

• scalar magnetometers measure the total strength of the

magnetic field to which they are subjected, and
• vector magnetometers have the capability to measure the
component of the magnetic field in a particular direction.

The use of three orthogonal vector magnetometers allows the

magnetic field strength, inclination and declination to be uniquely
defined. Examples of vector magnetometers are fluxgates and
superconducting quantum interference devices, or SQUIDs. Some
scalar magnetometers are discussed below.

A magnetograph is a special magnetometer that continuously

records data.
Fig: outline diagram of magnetometer

The NAV420CA GPS-based inertial navigation system

The NAV420CA uses angular rate sensors and linear

acceleration sensors that are bulk machined MEMS devices.
The NAV420CA also incorporates many new features:
• Built in GPS receiver for position and velocity measurement.
• GPS data synchronization clock.
 • High performance Kalman filters algorithms.
• Water resistant, vibration resistant light weight design.
• EMI protection for trouble-free operation.
• Continuous built-in test.

The NAV420 provides consistent performance over a wide

temperature range in challenging EMI environments across a broad
range of input power conditions. It is designed for use in a number of
different applications including unmanned vehicle control, land
vehicle guidance, uncertified avionics and platform stabilization.
This high reliability, strap down inertial system provides attitude
and heading measurement with static and dynamic accuracies that
exceed traditional spinning mass vertical and directional gyros. With
GPS integration, the NAV420 system also provides GPS-aided
velocity data at up to 100 Hz. Velocity data derived from the inertial
instruments improves stability and reduces the latency associated
with stand-alone GPS measurements.

NAV420CA Architecture:
The NAV420CA is an intelligent attitude gyro for roll, pitch and
directional gyro for heading angle measurements in dynamic
environments. The unit is also a nine-axis measurement system that
outputs accurate acceleration, angular rates and magnetic
orientation. The NAV420CA uses the latest in solid-state sensor
technology resulting in superior performance, reliability and stability
over time and operating environments.
The NAV420CA consists of the following subsystems:
1) Inertial Sensor Array: This is an assembly of three accelerometers,
three gyros (rate sensors) and four temperature sensors.
2) A three axis fluxgate magnetometer board used to compute
heading.
3) A WAAS capable GPS receiver for position and velocity
measurement.
4) A digital signal processing (DSP) module, which receives the
signals from the inertial sensors and magnetometers. This unit
converts these signals to digital data, filters the data, computes the
attitude solution, monitors and processes all BIT data, and transmits
the results to the user.

x/y/z
Gyros Digital Signal
(MEMS) RS-232
Processor
+
16 Bit
x/y/z Sensor
A/D
Accelerom Compensatio
converter
eters n+
Full State
Kalman Filter
x/y/z
magnetome
ters

Power
Temperature GPS receiver
Conditioning

Fig: Block Diagram of NAV420CA

The NAV420CA analog sensor signals are sampled and

converted to digital data at 1 kHz. The sensor data is filtered and
down-sampled by a DSP using FIR filters. Factory calibration data,
stored in EEPROM, is used by the DSP to remove temperature bias,
misalignment, scale factor errors, and non-linearities from the sensor
data. The firmware inside the onboard processors produces
calibrated angular rate measurements, calibrated acceleration
measurements, calibrated magnetometer measurements, and the
estimated navigation state which includes body attitude (roll, pitch,
heading), local level horizontal navigation frame position (latitude,
longitude, and altitude) and velocity (north, east, down). The
algorithm used to estimate the navigation state is an Extended
Kalman Filter (EKF) trajectory correction approach in which the
inertial accelerometers and gyros propagate the state trajectory made
up of velocity, and body attitude, and the supporting sensors (GPS
and magnetometers) provide velocity and earth magnetic field
measurements which the filter uses to calculate corrections to the
trajectory state, and estimate inertial sensor errors and ferrous
material effects. The DSP performs time-triggered trajectory
propagation at 100Hz and will synchronize the sensor sampling with
the GPS UTC (Universal Coordinated Time) second boundary when
available.

Attitude and Heading Determination Algorithm

The attitude and heading determination algorithm is divided into
two separate entities. The gyro measured angular rate information is
integrated in an attitude trajectory propagation model, and attitude
errors and gyro biases are estimated in a Kalman Filter Attitude and
Heading Estimation Model. In general, gyros and accelerometers
suffer from bias drift, misalignment errors, acceleration errors (g-
sensitivity), nonlinearity (square terms), and scale factor errors. The
magnetometers are also susceptible to magnetic disturbances, which
corrupt their measurement of the earth magnetic field. These errors,
typically known as hard iron and soft iron effects, are calibrated out
once the system is installed in its final mounting position. The largest
error in attitude and heading propagation is associated with the gyro
bias terms. The Kalman filter attitude estimation component provides
an on-the-fly calibration for the gyros by providing corrections to the
attitude processor trajectory and a characterization of the gyro bias
state. The accelerometers provide an attitude reference using gravity,
and the magnetometers provide a heading reference using the
earth’s magnetic field vector. The NAV420CA runs a 7 state AHRS
type EKF when GPS signal is unavailable and a 13 state reduced
navigation filter (loosely coupled) when GPS signal is available. The
errors resulting due to double integration of MEMS inertial sensors
makes position tracking very difficult and hence, only velocities are
tracked in the filter. The tangent frame is used as the navigation
frame and earth rate is not considered. By reducing the navigation
filter to this minimalist form, we are able to sample the sensor set at 1
kHz and run the navigation filter at 100 Hz.

Kalman Filter Attitude and Heading Estimation Model

The Kalman filter attitude correction approach achieves

improved performance due to its ability to estimate the attitude errors
and gyro bias states. The advantage with this approach is that an
absolute attitude error estimate is provided to the trajectory to correct
any errors due to physical noise disturbances and gyro errors, as well
as a characterization and “tracking” of the gyro biases which in effect
provides an online rate sensor calibration. The filter model is an
Extended Kalman Filter formulation made up of two components, a
linearized attitude error and gyro bias state model, and a nonlinear
attitude quaternion error measurement model. The state model
predicts where the attitude errors and gyro bias states will propagate
based on input data from the gyros, and the measurement model
corrects this prediction with the real world attitude error
measurements obtained from the accelerometer gravity and
magnetometer earth magnetic field reference. This balance of state
modeling with real world observables gives the Kalman filter the
adaptive intelligence to assign appropriate confidence levels on its
two components. The NAV420CA blends GPS, magnetometer, and
accelerometer measurements into the EKF update depending on the
health and status of the associated sensors. The NAV420CA uses the
GPS receiver’s tangent frame velocity estimate as a direct
measurement update for the EKF. In this way, the NAV420CA utilizes
the GPS receiver’s internal ECEF to tangent frame velocity
transformation resulting in a computationally efficient measurement
update. The NAV420CA uses its internal magnetometers to provide a
heading measurement update for the EKF. Magnetometer
measurements are calibrated, corrected for hard iron and soft iron
effects from the local environment, and rotated to the tangent frame.
The World Magnetic Model (WMM) 2005 is used to calculate the
relationship between magnetometer heading and true heading,
known as magnetic declination or variance. GPS position is used to
calculate the tangent frame earth magnetic field vector from the
WMM and the declination angle is computed from the WMM
horizontal components. The magnetometer measurements can then
be rotated through the declination angle to provide a true heading
measurement. The true heading measurement’s nonlinear
relationship to the quaternion attitude is captured by the EKF
measurement model to create a linearized filter update. If the GPS
link is lost or poor, the Kalman Filter solution stops tracking
accelerometer bias. The algorithm continues to apply gyro bias
correction and provides stabilized angle outputs. The EKF tracking
states are reduced to angles and gyro bias only. The accelerometers
will continue to integrate velocity, however, accelerometer noise, bias,
and attitude error will cause the velocity estimates to walk off on the
order of seconds. The attitude tracking performance will degrade and
the filter will become susceptible to “false gravity” acceleration errors,
typical of the AHRS only EKF formulation. The UTC packet
synchronization will drift due to internal clock drift.

The NAV420CA is bundled with software to view the data sent

by the device on the computer screen in .txt format. The format of the
data present in the log file is given figure-
The control panel of the NAV view software is shown in the figure

From the software we can also view the graph of roll, pitch; yaw
etc.We can also view the fast Fourier transform and the actual
navigation panel. The pictures are shown
Fig: graph of roll, pitch and yaw when the system is steady.
Fig: graph showing roll, pitch and yaw when the system is not steady.
Fig: Fast Fourier Transformation when static
Fig: Fast Fourier Transformation when not static.
Fig: Navigation Display panel

Calibration Procedure:

Before using the device it has to be calibrated the process of

calibration along with proper figure are given below:
1. Apply power to the NAV420CA.
2. Start NAV-VIEW software and make sure communication is
established.
3. Wait 60 seconds for initialization to complete.
4. Click “PRESS CAL” button on the NAV-VIEW

5. The following message will appear, click “Start” to begin the

calibration.

6. Slowly rotate the device on which the system is mounted

through a 380°turn (10-20 degrees per second is ideal) until the
following message appears.
7. Stop the turn and write down the three calibration parameters
displayed. These numbers represent the magnetic environment
around the NAV420CA.
8. Click “accept” to store the calibration or “cancel” to ignore the
calibration.
9. Disconnect power from the NAV420CA.
10.Wait 10 seconds.
11. Repeat this calibration procedure by going to step-2 to verify
consistency in the calibration parameters. Each parameter
should not deviate more than 0.01 between calibrations.
MEMS Technology:
Micro-electro-mechanical systems (MEMS) are small integrated
devices or systems that combine electrical and mechanical
components. They range in size from the sub micrometer (or sub
micron) level to the millimeter level and there can be any number,
from a few to millions, in a particular system. MEMS extend the
fabrication techniques developed for the integrated circuit industry to
add mechanical elements such as beams, gears, diaphragms, and
springs to devices.

Examples of MEMS device applications include inkjet-printer

cartridges, accelerometers, miniature robots, micro engines, locks,
inertial sensors, micro transmissions, micro mirrors, micro actuators,
optical scanners, fluid pumps, transducers, and chemical, pressure
and flow sensors. New applications are emerging as the existing
technology is applied to the miniaturization and integration of
conventional devices.

These systems can sense, control, and activate mechanical

processes on the micro scale, and function individually or in arrays to
generate effects on the macro scale. The micro fabrication
technology enables fabrication of large arrays of devices, which
individually perform simple tasks, but in combination can accomplish
complicated functions.

MEMS are not about any one application or device, nor are
they defined by a single fabrication process or limited to a few
materials. They are a fabrication approach that conveys the
advantages of miniaturization, multiple components, and
microelectronics to the design and construction of integrated
electromechanical systems. MEMS are not only about miniaturization
of mechanical systems; they are also a new paradigm for designing
mechanical devices and systems.

Kalman Filter:
The Kalman filter is essentially a set of mathematical equations
that implement a predictor-corrector type estimator that is optimal in
the sense that it minimizes the estimated error covariance—when
some presumed conditions are met. Since the time of its introduction,
the Kalman filter has been the subject of extensive research and
application, particularly in the area of autonomous or assisted
navigation. This is likely due in large part to advances in digital
computing that made the use of the filter practical, but also to the
relative simplicity and robust nature of the filter itself. Rarely do the
conditions necessary for optimality actually exist, and yet the filter
apparently works well for many applications in spite of this situation.
The Kalman filter is named after Rudolph E. Kalman, who in 1960
published his famous paper describing a recursive solution to the
discrete-data linear filtering problem.
Fig: Typical Kalman filter Application

The Kalman filter theory and algorithm:

Suppose we have a linear system model as described

previously. We want to use the available measurements y to estimate
the state of the system x. We know how the system behaves
according to the state equation, and we have measurements of the
position, so how can we determine the best estimate of the state x?
We want an estimator that gives an accurate estimate of the true
state even though we cannot directly measure it. What criteria should
our estimator satisfy? Two obvious requirements come to mind. First,
we want the average value of our state estimate to be equal to the
average value of the true state. That is, we don’t want our estimate to
be biased one way or another. Mathematically, we would say that the
expected value of the estimate should be equal to the expected value
of the state. Second, we want a state estimate that varies from the
true state as little as possible. That is, not only do we want the
average of the state estimate to be equal to the average of the true
state, but we also want an estimator that results in the smallest
possible variation of the state estimate. Mathematically, we would say
that we want to find the estimator with the smallest possible error
variance. It so happens that the Kalman filter is the estimator that
satisfies these two criteria. But the Kalman filter solution does not
apply unless we can satisfy certain assumptions about the noise that
affects our system. Remember from our system model that w is the
process noise and z is the measurement noise. We have to assume
that the average value of w is zero and the average value of z is zero.
We have to further assume that no correlation exists between w and
z. That is, at any time k, wk and zk are independent random variables.
Then the noise covariance matrices Sw and Sz are defined as

Process noise covariance:

Sw =E (wk wk T)
Measurement noise covariance:
Sz =E (zk zk T)

Where wT and zT indicate the transpose of the w and z random noise

vectors, and E(·) means the expected value.
Now we are finally in a position to look at the Kalman filter
equations. There are many alternative but equivalent ways to express
the equations. One of the formulations is given as follows:
Kk=APkCT ( CPkCT + Sz ) -1
xk+1=( Axk + Buk ) + Kk ( vk+1 - Cxk )
P k+1=APkAT - Sw - APkCTSz-1CPkAT

That’s the Kalman filter. It consists of three equations, each

involving matrix manipulation. In the above equations, a –1
superscript indicates matrix inversion and a T superscript indicates
matrix transposition. The K matrix is called the Kalman gain, and the
P matrix is called the estimation error covariance. The state estimate
(x) equation is fairly intuitive. The first term used to derive the state
estimate at time k+1 is just A times the state estimate at time k, plus
B times the known input at time k. This would be the state estimate if
we didn’t have a measurement. In other words, the state estimate
would propagate in time just like the state vector in the system model.
The second term in the equation is called the correction term and it
represents the amount by which to correct the propagated state
estimate due to our measurement. Inspection of the K equation
shows that if the measurement noise is large, Sz will be large, so K
will be small and we won’t give much credibility to the measurement y
when computing the next. On the other hand, if the measurement
noise is small, Sz will be small, so K will be large and we will give a lot
of credibility to the measurement when computing the next.
 Input to plant
New control enters here e.g.
Prediction is steering wheel
last estimate u(k) available?

x(k|k-1)=Fx(k|k-1)+Bu(k)
x(k|k-1)=x(k-1|k-1) P(k|k-1)=F P(k|k-
P(k|k-1)=P(k-1|k-1) 1)FT+GQGT

Data from
sensors enter
New Observation z algorithm here
(k) available
Delay

x(k|k)=x(k|k-1) S=H P(k|k-1)H T+R

W=P(k|k-1)HTS-1
P(k|k)=P(k|k-1) v(k)=z(k)-Hx(k|k-1)

Estimate is
last

k=k+1
x(k|k)=x(k|k-1)+Wv(k)
P(k|k)=P(k|k-1)-W S W T

Fig: Kalman Filtering Algorithm

 Introduction to RS-232:

RS232 is the most known serial port used in transmitting the data in
communication and interface. Even though serial port is harder to program than the
parallel port, this is the most effective method in which the data transmission requires less
wires that yields to the less cost. The RS232 is the communication line which enables the
data transmission by only using three wire links. The three links provides ‘transmit’,
‘receive’ and common ground...

The ‘transmit’ and ‘receive’ line on this connecter send and receive data
between the computers. As the name indicates, the data is transmitted serially.
The two pins are TXD & RXD. There are other lines on this port as RTS, CTS,
DSR, DTR, and RTS, RI. The ‘1’ and ‘0’ are the data which defines a voltage
level of 3V to 25V and -3V to -25V respectively.

The electrical characteristics of the serial port as per the EIA (Electronics
Industry Association) RS232C Standard specifies a maximum baud rate of
20,000bps, which is slow compared to today’s standard speed. For this reason,
we have chosen the new RS-232D Standard, which was recently released.

The RS-232D has existed in two types. i.e., D-TYPE 25 pin connector
and D-TYPE 9 pin connector, which are male connectors on the back of the PC.
You need a female connector on your communication from Host to Guest
computer. The pin outs of both D-9 & D-25 are show below.

D-Type-9 D-Type-25 pinPin outs Function

pin no. no.

3 2 RD Receive Data (Serial data input)

2 3 TD Transmit Data (Serial data output)
7 4 RTS Request to send (acknowledge to modem that
UART is ready to exchange data
8 5 CTS Clear to send (i.e.; modem is ready to exchange
data)
6 6 DSR Data ready state (UART establishes a link)
5 7 SG Signal ground
1 8 DCD Data Carrier detect (This line is active when
modem detects a carrier
4 20 DTR Data Terminal Ready.
9 22 RI Ring Indicator (Becomes active when modem
detects ringing signal from PSTN
About DTE & DCE:

Devices, which use serial cables for their communication, are split into two
categories. These are DCE (Data Communications Equipment) and DTE (Data
Terminal Equipment.) Data Communications Equipments are devices such as
your modem, TA adapter, plotter etc while Data Terminal Equipment is your
Computer or Terminal. A typical Data Terminal Device is a computer and a typical
Data Communications Device is a Modem. Often people will talk about DTE to
DCE or DCE to DCE speeds. DTE to DCE is the speed between your modem
and computer, sometimes referred to as your terminal speed. This should run at
faster speeds than the DCE to DCE speed. DCE to DCE is the link between
modems, sometimes called the line speed.

Most people today will have 28.8K or 33.6K modems. Therefore, we

should expect the DCE to DCE speed to be either 28.8K or 33.6K. Considering
the high speed of the modem we should expect the DTE to DCE speed to be
about 115,200 BPS. (Maximum Speed of the 16550a UART) . The
communications program, which we use, has settings for DCE to DTE speeds.
However, the speed is 9.6 KBPS, 144 KBPS etc and the modem speed.

If we were transferring that text file at 28.8K (DCE- DCE), then when the
modem compresses it you are actually transferring 115.2 KBPS between
computers and thus have a DCE- DTE speed of 115.2 KBPS. Thus, this is why
the DCE- DTE should be much higher than the modem's connection speed.
Therefore, if our DTE to DCDCE speed is several times faster than our DCE to
DCE speed the PC can send data to your modem at 115,200 BPS.

How to program the Serial Port using C/C++?

There are two popular methods of sending data to or from the serial port in
Turbo C. One is using outportb(PORT_ID, DATA) or outport(PORT_ID,DATA)
defined in “dos.h”. Another method is using bioscom() function defined in “bios.h”.

Using outportb() :

The function outportb () sends a data byte to the port ‘PORT_ID’. The
function outport() sends a data word. These functions can be used for any port
including serial port, parallel ports. Similarly to receive data these are used.

• inport reads a word from a hardware port

• inportb reads a byte from a hardware port
• outport outputs a word to a hardware port
• outportb outputs a byte to a hardware port

Declaration:
 • int inport(int portid);
• unsigned char inportb(int portid);
• void outport(int portid, int value);
• void outportb(int portid, unsigned char value);

Remarks:

• inport works just like the 80x86 instruction IN. It reads the
low byte of a word from portid, the high byte from portid + 2.
• inportb is a macro that reads a byte
• outport works just like the 80x86 instruction OUT. It writes
the low byte of value to portid, the high byte to portid + 1.
• outportb is a macro that writes value Argument

portid:

• Inport- port that inport and inportb read from;

• Outport- port that outport and outportb write to

value:

• Word that outport writes to portid;

• Byte- that outportb writes to portid.

If you call inportb or outportb when dos.h has been included, they are
treated as macros that expand to inline code.

If you don't include dos.h, or if you do include dos.h and #undef the
macro(s), you get the function(s) of the same name.

Return Value:

# inport and inportb return the value read

# outport and outportb do not return

For more details of these functions read article from beondlogic.com

Using bioscom:

The macro bioscom () and function _bios_serialcom() are used in this

method in the serial communication using RS-232 connecter. First we have to set
the port with the settings depending on our need and availability. In this method,
same function is used to make the settings using control word, to send data to
the port and check the status of the port. These actions are distinguished using
 the first parameter of the function. Along with that we are sending data and the
port to be used to communicate.

Here are the deatails of the Turbo C Functions for communication

ports.

Declaration:

bioscom(int cmd, char abyte, int port)

_bios_serialcom(int cmd ,int port, char abyte)

bioscom() and _bios_serialcom() uses the bios interrupt 0x14 to perform

various communicate the serial communication over the I/O ports given in port.

cmd: The I/O operation to be performed.

cmd (boiscom) cmd(_bios_serialcom)Action

0 _COM_INIT
1 _COM_SEND
2 _COM_RECEIVE
3 _COM_STATUS
Initialise the parameters to the port
Send the character to the port
Receive character from the port
Returns rhe current status of the
communication port

portid: port to which data is to be sent or from which data is to be read.

0: COM1
1: COM2
2: COM3

abyte:

When cmd =2 or 3 (_COM_SEND or _COM_RECEIVE) parameter abyte is

ignored.

When cmd = 0 (_COM_INIT), abyte is an OR combination of the following bits

(One from each group):

value of abyte Meaning

Bioscom _bios_serialcom

0x02 _COM_CHR7 7 data bits

0x03 _COM_CHR8 8 data bits

0x00 _COM_STOP1 1 stop bit
 _COM_STOP2
0x04 2 stop bits
0x00 _COM_NOPARITY No parity

0x08 _COM_ODDPARITY Odd parity

0X10 _COM_EVENPARITY Even parity

0x00 _COM_110 110 baud

0x20 _COM_150 150 baud

0x40 _COM_300 300 baud

0x60 _COM_600 600 baud

0x80 _COM_1200 1200 baud

0xA0 _COM_2400 2400 baud

0xC0 _COM_4800 4800 baud

0xE0 _COM_9600 9600 baud

For example, if

abyte = 0x8B = (0x80 | 0x08 | 0x00 | 0x03) = (_COM_1200 |

_COM_ODDPARITY | _COM_STOP1 | _COM_CHR8)

the communications port is set to

1200 baud (0x80 = _COM_1200)
Odd parity (0x08 = _COM_ODDPARITY)
1 stop bit (0x00 = _COM_STOP1)
8 data bits (0x03 = _COM_CHR8)

To initialise the port with above settings we have to write,

bioscom(0, 0x8B, 0);

To send a data to COM1, the format of the function will be bioscom(1, data, 0).
Similarly bioscom(1, 0, 0 ) will read a data byte from the port.

Apart from this the commands which I have used in my program includes
the library function “windows.h”.This library function is used to read the