Dsp kit implementation of Kalman filter based sensor fused algorithm for attitude sensors

1
DSP Implementation of Kalman Filter based Sensor Fused
Algorithm for Attitude sensors
Final Year Design Project Report
Submitted by
Muhammad Salman 2009186
Myra Aslam 2009213
Umar Farooq 2009296
Walya Sadiq 2009311
Zorays Khalid 2009327
Advisor
Dr. Adnan Noor
Co-Advisor
Dr. Ziaul Haq Abbas
Faculty of Electronic Engineering
Ghulam Ishaq Khan Institute of Engineering Sciences and Technology.

2
ACKNOWLEDGEMENT
The work done in this project would not have been possible without the help, guidance and
constant motivation by Dr. Adnan Noor, Dr. Ziaul Haq Abbas, Mr. Mazhar Javed, Mr. Saad
Sardar (Suparco). I would also like to acknowledge Mr. Abdullah Jan, Mr. Gul Hanif, Mr.
Iftikhar, Mr. Javed, Mr. Tanvir, Mr.Ahsan Farooqui, Mr.Usman Salauddin, Mr. Ahsan Abdullah
and all the entire Faculty of Electronics Engineering and Suparco for their support and guidance.

3
ABSTRACT
There is, at present, a very limited diversity in aircraft IMU's capable of correct attitude sensing.
In real environments, it is difficult to attain attitude readings what are desired due to real time
disturbances. The task incorporates the implementation of the attitude estimation algorithm on a
DSP platform. The objective of our project is to generate properly formatted code and propose
the optimization of the code for platform specific requirements. This task is achieved through the
use of MEMS based and COTS sensors, microcontrollers and Digital Signal Processing Kit. The
IMU along with celestial sensor is used to sense the change in orientation of the platform which
is developed for verification and then process the positions in microcontrollers and display the
accurate results on Graphical User Interface.

4
LIST OF ABBREVIATIONS
Abbreviations:
IMU - Inertial Measurement Unit
MEMS - Micro Electronic Mechanical System
Bps - Bits per Second
Dps- Degrees per second
Rbi- Rotation Matrix body inertial reference frame
Rbs- Rotation matrix between body and sun sensor reference frame
Accel- Accelerometer
Gyro- Gyroscope
Magneto- Magnetometer
INS- Inertial navigation system

5
TABLE OF CONTENTS
ABSTRACT-----------------------------------------------------------------------------------------------------------------3
LIST OF ABBREVIATIONS-------------------------------------------------------------------------------------------------4
CHAPTER 1- INTRODUCTION--------------------------------------------------------------------------------------6
1.1 Objective-------------------------------------------------------------------------------------------------------6
1.2 Motivation-----------------------------------------------------------------------------------------------------6
1.3 Our line of work-----------------------------------------------------------------------------------------------6
CHAPTER 2- INERTIAL MEASUREMENT UNIT------------------------------------------------------------------------8
2.1 Gyroscope-------------------------------------------------------------------------------------------------------------9
2.2 Magnetometer----------------------------------------------------------------------------------------------------------11
2.3 Accelerometer--------------------------------------------------------------------------------------------------13
2.4 Calibration -------------------------------------------------------------------------------------------------------------17
CHAPTER3-CELESTIAL OBJECT SENSOR ------------------------------------------------------------------------19
3.1 Principle -------------------------------------------------------------------------------------------------------19
CHAPTER 4-THE KALMAN FILTER-----------------------------------------------------------------------------22
4.1 Principle ------------------------------------------------------------------------------------------------------22
4.2 Applications --------------------------------------------------------------------------------------------------22
4.3 Graphical Results -------------------------------------------------------------------------------------------24
CHAPTER 5- ASSEMBLY AND DESIGN-------------------------------------------------------------------------25
5.1 Introduction--------------------------------------------------------------------------------------------------25
5.2 Stepper Motor ------------------------------------------------------------------------------------------------25
5.3 Embedded System--------------------------------------------------------------------------------------------25
5.4 Mechanical Model----------------------------------------------------------------------------------------------25
CHAPTER 6-ARDUINO MICROCONTROLLER ---------------------------------------------------------------------27
6.1 Message protocol (I2C) ---------------------------------------------------------------------------------------------27
6.2 Connection of Arduino board to 10 DOF IMU-----------------------------------------------------------------------28
6.3 Results displayed on Serial Monitor -----------------------------------------------------------------------------------28
CHAPTER 7- CONCLUSION--------------------------------------------------------------------------------------29
7.1 Future Enhancement------------------------------------------------------------------------------------------29
REFERENCES--------------------------------------------------------------------------------------------------------30
APPENDIX A---------------------------------------------------------------------------------------------------------31
APPENDIX B---------------------------------------------------------------------------------------------------------44

6
CHAPTER 1
INTRODUCTION
1.1 OBJECTIVE
The objective of this project is the implementation of the attitude estimation algorithm (already
developed) on a DSP platform and optimizing its performance.
1.2 MOTIVATION
Nowadays, real time environment often prove unexpected results and have much variation in
idealistic behavior of the object. Automation is the most frequently spelled term in the fields of
electronics. The hunger for automation brought many revolutions in existing technologies. Real
time data logging of the unmanned space vehicles regarding attitude and various other
parameters like velocity, inclination, direction etc. takes place on board in such vehicles. great
interest shown by the aviation industry for unmanned aerial vehicles for a particular technique to
minimize measurement errors called, Kalman filtering and that truly motivated us to go on with
this project and the project will be useful in many ways as it has new algorithm implemented in
it which are much more efficient than in the past.
1.3 OUR LINE OF WORK
In the project, Kalman filter is adopted in form of a MATLAB code on the Kalman filter
algorithm. Hardware acquisition for framing inertial and body frame of reference from a celestial
object such as the sun is an essential part. We increase the efficiency of the algorithm for fusing
data coming from different sensors having different tolerances.

7
So our project has following major parts
 Controls
 Algorithm development
 Integration and interfacing
Figure 1.1- Block Diagram

8
CHAPTER 2
INERTIAL MEASUREMENT UNIT
An inertial measurement unit, or IMU, is an electronic device that measures and reports on a
craft's velocity, orientation and gravitational forces using a combination of accelerometer,
gyroscope and magnetometer. IMUs are typically used to maneuver aircraft, including unmanned
aerial vehicles (UAVs), among many others, and spacecraft, including satellites and landers.
Recent developments allow for the production of IMU-enabled GPS devices. An IMU allows a
GPS to work when GPS-signals are unavailable, such as in tunnels, inside buildings, or when
electronic interference is present.
IMU is the main component of INS and is used in aircraft, spacecraft, watercraft and guided
missiles. In this capacity, the data collected from the IMU's sensors allows a computer to track a
craft's position.
An IMU works by detecting the current rate of acceleration using one or more accelerometers,
and detects changes in rotational attributes like pitch, roll and yaw using one or more
gyroscopes.

9
Recently, more and more manufacturers also include magnetometers in IMUs. This allows better
performance for dynamic orientation calculation in Attitude and heading reference systems
which base on IMUs. We used 10 DOF IMU sensor;
2.1 GYROSCOPE
A gyroscope is a device for measuring or maintaining orientation, based on the principles of
angular momentum. In essence, a mechanical gyroscope is a spinning wheel or disk whose axle
is free to take any orientation. Gyroscopes based on other operating principles also exist, such as
in 10 DOF IMU, microchip-packaged MEMS gyroscope is found.

10
2.1.1 PRINCIPLE
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 is journaled
for oscillation in an outer gimbal for a total of two gimbals.
The outer gimbal or ring, which is the gyroscope frame, is mounted so as to pivot about an axis
in its own plane determined by the support. This outer gimbal possesses one degree of rotational
freedom and its axis possesses none. The next inner gimbal is mounted in the gyroscope frame
(outer gimbal) so as to pivot about an axis in its own plane that is always perpendicular to the
pivotal axis of the gyroscope frame (outer gimbal) as shown in figure. In MEMS based
gyroscope, a vibrating element made up of MEMS is used, thus smaller with greater sensitivity
and accuracy.
Figure: Gyro Wheel
2.1.2 PROPERTIES
The fundamental equation describing the behavior of the gyroscope is:
τ =
𝑑𝐋
𝑑𝑡
=
𝑑(𝐼𝜔)
𝑑𝑡
= Iα

11
where the pseudo vectors τ and L are, respectively, the torque on the gyroscope and its angular
momentum, the scalar I 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 L, results in a rotation about an axis perpendicular to both τ and L. This motion
is called precession. The angular velocity of precession ΩP is given by the cross product:
τ = ΩP x L
Under a constant torque of magnitude τ, the gyroscope's speed of precession ΩP is inversely
proportional to L, the magnitude of its angular momentum:
τ = ΩPLsinθ
Where θ is the angle between vectors ΩP and L. e.g. if the gyroscope's spin slows down (for
example, due to friction), its angular momentum decreases and so 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.
2.2 MAGNETOMETER
Compass works by aligning itself to the earth’s magnetic field. Because the compass' needle is a
ferrous material, it aligns swings on its bearing in the center as the magnetic field of the earth
pulls it into alignment. These magnetic fields expand throughout the surface of the earth (and
beyond) so we used them to help us tell us which direction we facing.

12
Our magnetometer uses these magnetic fields; however it doesn't pull on a little needle inside it!
(It probably wouldn't fit anyway). Inside our magnetometer are three magneto-resistive
sensors on three axis. The effect of magnetic fields on these sensors adjust the current flow
through the sensor. By applying a scale to this current, we can tell the magnetic force (measured
in Gauss) on this sensor. By combining information about two or more of these axis we started to
use the difference in the magnetic fields in the sensors to infer our bearing to magnetic north.
2.2.1 PRINCIPLE
A simple calclation we used to create a compass is below. When the device is level, (pitch (Xb)
and roll (Yb) are at 0 degrees). The compass heading can be determined like so:
The local earth magnetic field has a fixed component Hh on the horizontal plane pointing to the
earths magnetic north. This is measured by the magnetic sensor axis XM and YM (here named

13
as Xh and Yh). Using this we calculated the heading angle using this simple equation:
Heading = arctan(Yh/Xh)=Magnetic North
2.3 ACCELEROMETER
An accelerometer is a device that measures proper acceleration. However, the proper
acceleration measured by an accelerometer is not necessarily the coordinate acceleration (rate of
change of velocity). Instead, it is the acceleration associated with the phenomenon of weight
experienced by any test mass at rest in the frame of reference of the accelerometer device.
In 10 DOF IMU, MEMS based accelerometer is used to detect magnitude and direction of the
proper acceleration (or g-force), as a vector quantity, and can be used to sense orientation
(because direction of weight changes), coordinate acceleration (so long as it produces g-force or
a change in g-force), vibration, shock, and falling (a case where the proper acceleration changes,
since it tends toward zero).
2.3.1 PRINCIPLE
An accelerometer measures proper acceleration, which is the acceleration it experiences relative
to freefall and. Such accelerations are popularly measured in terms of g-force. An accelerometer
at rest relative to the Earth's surface will indicate approximately 1 g upwards, because any point
on the Earth's surface is accelerating upwards relative to the local inertial frame. To obtain the
acceleration due to motion with respect to the Earth, this "gravity offset" must be subtracted and
corrections for effects caused by the Earth's rotation relative to the inertial frame.

14
Acceleration is quantified in the SI unit meters per second per second (m/s2
), or popularly in
terms of g-force (g).
This is an illustration of an accelerometer; they are not actually
constructed this way, this is just for the purpose of
understanding. Here the mass (in blue) is suspended by four
springs attached to the frame. At the moment all these springs
are zero, which means no force is being applied to the mass
relative to the frame, but this is not actually what we see when
our accelerometer is placed on the desk.
We actually see something more like this:
This is because gravity is acting on the mass and is pulling it
down. The accelerometer is measuring 1 g because that is the
amount of gravity you experience on the surface of the earth.

15
The accelerometer also measures movement, so if we move the
accelerometer from side to side, the result looks like this.
2.3.2 STRUCTURE
Nowadays, accelerometers used are piezoelectric, piezoresistive and capacitive components
which are used to convert the mechanical motion into an electrical signal.
Modern accelerometers like one in 10 DOF IMU 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). Damping results from
the residual gas sealed in the device. As long as the Q-factor is not too low, damping does not
result in a lower sensitivity.
Under the influence of external accelerations the proof mass deflects from its neutral position.
This deflection is measured in an analog or digital manner. Most commonly, the capacitance
between a set of fixed beams and a set of beams attached to the proof mass is measured. This
method is simple, reliable, and inexpensive.

16
Micromechanical accelerometers are available in a wide variety of measuring ranges, reaching
up to thousands of g's. The designer must make a compromise between sensitivity and the
maximum acceleration that can be measured.
2.3.3 THE TILT PROBLEM
A problem that traditional compasses have is that they need to be held flat to function. If you
hold a compass at right angles it will not work at all, and if you tilt it to 45 degrees the reading
will be more inaccurate the further the compass is tilted. This problem occurs because the
compass is only using the X and Y axis of the earth’s magnetic field (the compass needle is fixed
onto a bearing that will only allow the needle to swivel on one axis). When the compass is not
parallel to these axis the amount of magnetism felt by the needle will change based on how out
of alignment the compass is to these axis.
We were able to compensate our compass for tilt up to 40 degrees, for which we needed a way to
include in our calculations the third axis, Z, which (when tilted) now collects the magnetic field
lost by X and Y when they are tilted out of alignment.
We first needed to know how the device is tilted, so we know how to integrate the Z axis
measurement properly, thus correct for our tilt. In other words, we needed to know our
orientation first. We did this by incorporating a triple axis accelerometer into our compass
system.

17
2.3.4 TILT COMPENSATION EQUATION
When the device is tilted, pitch and roll angles are not 0°. In the diagram below, the pitch and
roll angles are measured by a 3 axis accelerometer. XM, YM and ZM (the measurement axis on the
magnetometer) will be compensated to obtain Xh and Yh. Once have corrected the Xh and
Yh measurements, we can use the first equation to calculate our heading.
Xh = XM * cos(Pitch) + ZM * sin(Pitch)
Yh = XM * sin(Roll) * sin(Pitch) + YM * cos(Roll) - ZM * sin(Roll) * cos(Pitch)
Tilt Compensated Heading = arctan(Yh/Xh)=Tilt Compensated Magnetic North
2.4 CALIBRATION
There are a variety of calibration procedures that can be performed ranging from very simple to
more complex. Possible calibration options are:
1. Rate gyro bias calibration
2. Rate gyro scale factor calibration
3. Accelerometer bias calibration
4. Magnetometer soft and hard iron calibration
5. Accelerometer and magnetometer reference vector adjustment

18
6. Accelerometer and rate gyro cross-axis misalignment correction
Details about the calibration procedure which we did are given below.
2.4.1 GYRO AND ACCELERO CALIBRATION
The zero-rate output the rate gyros and the zero-acceleration output of the accelerometers is
device-dependent. On the 10 DOF IMU, the angular rates used for angle estimation are given by
rate = R_gyro*(measured_rate - bias)
where R_gyro is a calibration matrix that scales the measurements and performs axis alignment,
measured_rate is a vector of raw data returned by the rate gyros, and bias is a vector of estimated
biases for each rate gyro.
Automatic gyro calibration is triggered by sending a ZERO_RATE_GYROS command to the
AHRS. During automatic calibration, which takes approximately three seconds, the AHRS
should be stationary. The gyro calibration matrix is, by default, diagonal with the diagonal
elements corresponding to scale factors that convert raw rates to actual angular rates in degrees
per second. The equation describing accelerometer correction is identical to that of the rate gyro
equation, but with unique biases and a unique calibration matrix.
2.4.2 MAGNETO HARD AND SOFT IRON CALIBRATION
Metal and magnetic objects near the 10 DOF IMU distort magnetometer measurements, creating
significant errors in estimated angles. Distortions from objects that are not in a fixed position
relative to the 10 DOF IMU cannot generally be corrected. However, distortions from objects
that are in a fixed position with respect to the sensor can be detected and corrected.

19
CHAPTER 3
CELESTIAL OBJECT SENSOR
A sun sensor is a device for measuring orientation, based on the location of sun relative to the
position of the stationary celestial object. Sun sensors are used because of their relative
simplicity and the fact that virtually all spacecraft use sun sensors of some type. The sun is a
useful reference direction because of its brightness relative to other astronomical objects, and its
relatively small apparent radius as viewed by a spacecraft near the Earth. Here we used Analog
sun sensors, which are based on photocells whose current output is proportional to the cosine of
the angle α between the direction to the sun and the normal to the photocell (Fig. 3.1). That
is, the current output is given by I(α) = I(0) cos α
Fig. 3.1
3.1 PRINCIPLE
The object of a sun sensor is to provide an approximate unit vector, with respect
to the body reference frame, that points towards the sun. We denote this vector by ŝ which can be
written as ŝ =si
T
{i}

20
Si , the unit vector direction to the sun in the Earth-centered inertial frame can be found with the
help of JD .
To determine the angle in a specific plane, two sun sensors are tilted at an angle α with respect to
the normal ń of the sun sensor (Fig. 3.2). This arrangement gives the angle between the sun
sensor normal, ń and the projection of the sun vector ŝ onto the ń – ť plane.
Fig. 3.2
Then the two photocells generate currents, using two appropriately arranged pairs of photocells
we obtain the geometry shown in Fig. 3.3. In this picture, ń1 is the normal vector for the first pair
of photocells, and ń2 is the normal vector for the second pair. The ť vector is chosen to define the
two planes of the photocell pairs.
Fig 3.3

21
3.1.1 CALCULATION OF Ss
Ss is calculated with the help of the information we get from hardware , that is the current values
from the four sensors , the angle α at which they are mounted , the maximum current intensity
etc
3.1.2 CALCULATION OF Rbs
Thus {ń1; ń2; ť} comprise the three unit vectors of a frame denoted by Fs (s for sun sensor). The
spacecraft designer determines the orientation of this frame with respect to the body frame that is
find the three rotational angles
th1, around z axis,
th2, around x axis,
th3, around y axis thus the orientation matrix Rbs is known.
3.1.3 CALCULATION OF Sb
The components of the sun vector in the sun sensor frame, Ss, is found using the two angles from
the sensors , then using the formula Sb = Rbs*Ss we find the components of ŝ in the body frame,
Sb.
3.1.4 CALCULATION OF Rbi (combined with magneto)
Using appropriate calculations Rbi the rotation matrix between body frame of reference and
inertial frame of reference is found
3.1.5 CALCULATION OF Rbi (not combined with magneto)
Rbi=Sb*inv(Si)

22
CHAPTER 4
KALMAN FILTER
The Kalman filter, also known as linear quadratic estimation (LQE), is an algorithm that uses
a series of measurements observed over time, containing noise (random variations) and other
inaccuracies, and produces estimates of unknown variables that tend to be more precise than
those based on a single measurement alone. More formally, the Kalman filter operates
recursively on streams of noisy input data to produce a statistically optimal estimate of the
underlying system state. The filter is named for Rudolf (Rudy) E. Kálmán, one of the primary
developers of its theory.
4.1 PRINCIPLE
The algorithm works in a two-step process. In the prediction step, the Kalman filter produces
estimates of the current state variables, along with their uncertainties. Once the outcome of the
next measurement (necessarily corrupted with some amount of error, including random noise) is
observed, these estimates are updated using a weighted average, with more weight being given to
estimates with higher certainty. Because of the algorithm's recursive nature, it can run in real
time using only the present input measurements and the previously calculated state; no additional
past information is required.
4.2 APPLICATIONS
The Kalman filter has numerous applications in technology. A common application is for
guidance, navigation and control of vehicles, particularly aircraft and spacecraft. Furthermore,

23
the Kalman filter is a widely applied concept in time series analysis used in fields such as signal
processing and econometrics.
4.2.1 EXAMPLE APPLICATION
As an example application, consider the problem of determining the precise location of a truck.
The truck can be equipped with a GPS unit that provides an estimate of the position within a few
meters. The GPS estimate is likely to be noisy; readings 'jump around' rapidly, though always
remaining within a few meters of the real position. In addition, since the truck is expected to
follow the laws of physics, its position can also be estimated by integrating its velocity over time,
determined by keeping track of wheel revolutions and the angle of the steering wheel. This is a
technique known as dead reckoning. Typically, dead reckoning will provide a very smooth
estimate of the truck's position, but it will drift over time as small errors accumulate.
In this example, the Kalman filter can be thought of as operating in two distinct phases: predict
and update. In the prediction phase, the truck's old position will be modified according to the
physical laws of motion (the dynamic or "state transition" model) plus any changes produced by
the accelerator pedal and steering wheel. Not only will a new position estimate be calculated, but
a new covariance will be calculated as well. Perhaps the covariance is proportional to the speed
of the truck because we are more uncertain about the accuracy of the dead reckoning estimate at
high speeds but very certain about the position when moving slowly. Next, in the update phase, a
measurement of the truck's position is taken from the GPS unit. Along with this measurement
comes some amount of uncertainty, and its covariance relative to that of the prediction from the
previous phase determines how much the new measurement will affect the updated prediction.
Ideally, if the dead reckoning estimates tend to drift away from the real position, the GPS

24
measurement should pull the position estimate back towards the real position but not disturb it to
the point of becoming rapidly changing and noisy.
4.2.1.1 MATHEMATICAL MODELING
4.3 GRAPHICAL RESULTS

25
CHAPTER 5
ASSEMBLY AND DESIGN
5.1 INTRODUCTION
The assembly consists of various components which includes pair of servo motors, Micro
controller and their trainer boards and testing platform.
5.2 STEPPER MOTOR
Stepper motors often come without datasheets, so therefore we had to find its full step sequence
through hit and trial method which required microcontroller burning and testing multiple number
of times before we arrived at a finally correct sequence. Once found, the code was adjusted with
appropriate step angles, calculations were performed and delays were adjusted accordingly and
the motor was controlled through port switches.
5.3 EMBEDDED SYSTEM
A PCB was designed and manufactured for microcontroller to run the firmware. The PCB was dedicated
to interface the stepper motor and essential circuitry to run the microcontroller was included.
5.4 MECHANICAL MODEL
The project’s main aim was to stabilize a platform and for this purpose mechanical assembly
supporting the desired task was required by making a Pan-Tilt assembly composing of two
servos which were used in such a manner that it enables rotation in three axes namely Yaw, Pitch

26
and Roll as shown in figure 3.2. IMU has been mounted on the motor through an acrylic sheet
which also holds the sun sensors on both sides at 45 degrees.
Figure 5.1: Test Platform

27
CHAPTER 6
ARDUINO MICROCONTROLLER
The Arduino Uno has a number of facilities for communicating with a computer and with IMU.
The Arduino software includes a serial monitor which allows simple textual data to be sent to
and from the Arduino board. The Rx and Tx LEDs on the board will flash when data is being
transmitted via the USB-to-serial chip and USB connection to the computer. Arduino board uses
I2C protocol for communicating with the 10 DOF IMU Sensor.
6.1 MESSAGE PROTOCOL (I2C)
I2C defines basic types of messages, each of which begins with a START and ends with a STOP:
 Single message where a master writes data to a slave;
 Single message where a master reads data from a slave;
 Combined messages, where a master issues at least two reads and/or writes to one or more
slaves.
In a combined message, each read or write begins with a START and the slave address. After the
first START, these are also called repeated START bits; repeated START bits are not preceded
by STOP bits, which is how slaves know the next transfer is part of the same message.
Any given slave will only respond to particular messages, as defined by its product
documentation

28
10 DOF IMU acts as a slave to the Arduino microcontroller, which acts as a master to the IMU.
6.2 CONNECTION OF ARDUINO BOARD TO 10 DOF IMU
6.3 RESULTS DISPLAYED ON SERIAL MONITOR
NOTE: These results were then further calibrated and magnetometer readings were tilt compensated to get accurate
angles.

29
CONCLUSION
The objectives set in the start were successfully achieved in FYP. A hardware platform with
IMU and sun sensors mounted upon it was developed along with the motor that enabled the
real time acquisition of yaw pitch and roll with high degree of accuracy. The output of
Arduino after stabilization was seen through MATLAB . Rotation in all 3 axis was tested to
cross check with our calculated values of the corresponding angles. The prototype was found
completely fulfilling varying disturbance produced by taking platform in hand.
FUTURE ENHANCEMENTS
 Use of multiple COTS.
 Use of Celestial Object Reference frame from fixed stars.

30
REFERENCES
[1] Guide to using IMU (Accelerometer and Gyroscope Devices) in
Embedded Applications: Starlino Electronics
[2] Attitude Determination: Chris Hall March18, 2003
[3] Texas Instruments, “Accelerometer”
[4] http://en.wikipedia.org/wiki/Accelerometer
[5] http://en.wikipedia.org/wiki/Gyroscope
[6] http://en.wikipedia.org/wiki/magnetometer
[7] http://www.arduino.cc/
[8] Aman Mangal-IIT Bombay, “Serial Communication between Arduino and
MATLAB”
[9] http://www.pololu.com/file/download/L3G4200D.pdf
[10] http://en.wikipedia.org/wiki/Flight_control_surfaces

31
APPENDIX A
Accelerometer
/************************************************************************
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU License V2. *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License, version 2 for more details *
* *
* Bare bones ADXL345 i2c example for Arduino 1.0 *
* by Jens C Brynildsen <http://www.flashgamer.com> *
* This version is not reliant of any external lib *
* (Adapted for Arduino 1.0 from http://code.google.com/p/adxl345driver)*
* *
* Demonstrates use of ADXL345 (using the Sparkfun ADXL345 breakout) *
* with i2c communication. Datasheet: *
* http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf *
* If you need more advanced features such as freefall and tap *
* detection, check out: *
* https://github.com/jenschr/Arduino-libraries *
***********************************************************************/
// Cabling for i2c using Sparkfun breakout with an Arduino Uno / Duemilanove:
// Arduino <-> Breakout board
// Gnd - GND
// 3.3v - VCC
// 3.3v - CS
// Analog 4 - SDA
// Analog 5 - SLC
// Cabling for i2c using Sparkfun breakout with an Arduino Mega / Mega ADK:
// Arduino <-> Breakout board
// Gnd - GND
// 3.3v - VCC
// 3.3v - CS
// 20 - SDA
// 21 - SLC
#include <Wire.h>
#define DEVICE (0x53) // Device address as specified in data sheet
byte _buff[6];
char POWER_CTL = 0x2D; //Power Control Register

32
char DATA_FORMAT = 0x31;
char DATAX0 = 0x32; //X-Axis Data 0
char DATAX1 = 0x33; //X-Axis Data 1
char DATAY0 = 0x34; //Y-Axis Data 0
char DATAY1 = 0x35; //Y-Axis Data 1
char DATAZ0 = 0x36; //Z-Axis Data 0
char DATAZ1 = 0x37; //Z-Axis Data 1
void setup()
{
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial for output. Make sure you set your
Serial Monitor to the same!
Serial.print("ninit n");
//Put the ADXL345 into +/- 4G range by writing the value 0x01 to the
DATA_FORMAT register.
writeTo(DATA_FORMAT, 0x01);
//Put the ADXL345 into Measurement Mode by writing 0x08 to the POWER_CTL
register.
writeTo(POWER_CTL, 0x08);
}
void loop()
{
readAccel(); // read the x/y/z tilt
delay(500); // only read every 0,5 seconds
}
void readAccel() {
uint8_t howManyBytesToRead = 6;
readFrom( DATAX0, howManyBytesToRead, _buff); //read the acceleration data
from the ADXL345
// each axis reading comes in 10 bit resolution, ie 2 bytes. Least
Significat Byte first!!
// thus we are converting both bytes in to one int
int x = (((int)_buff[1]) << 8) | _buff[0];
int y = (((int)_buff[3]) << 8) | _buff[2];
int z = (((int)_buff[5]) << 8) | _buff[4];
Serial.print("x: ");
Serial.print( x );
Serial.print(" y: ");
Serial.print( y );
Serial.print(" z: ");
Serial.println( z );
}
void writeTo(byte address, byte val) {
Wire.beginTransmission(DEVICE); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
// Reads num bytes starting from address register on device in to _buff array
void readFrom(byte address, int num, byte _buff[]) {

33
Wire.write(address); // sends address to read from
Wire.requestFrom(DEVICE, num); // request 6 bytes from device
int i = 0;
while(Wire.available()) // device may send less than requested
(abnormal)
{
_buff[i] = Wire.read(); // receive a byte
i++;
}
}
Gyroscope
//Arduino 1.0+ only
#include <Wire.h>
#define CTRL_REG1 0x20
int L3G4200D_Address = 105; //I2C address of the L3G4200D

34
int x = 0;
float p_z = 0;
float p_y = 0;
float p_x = 0;
int y;
int z = 0;
float degreesPerSecondz = 0;
float degreesPerSecondy = 0;
float degreesPerSecondx = 0;
float gyroRate = 0;
float currentAnglex = 0;
float currentAngley = 0;
float currentAnglez = 0;
float currentAngleDegrees = 0;
long currMillis = 0;
long pastMillis = 0;
long calibrationSumz = 0;
long calibrationSumy = 0;
long calibrationSumx = 0;
int gyroZeroz = 0;
int gyroHighz = 0;
int gyroLowz = 0;
int gyroZeroy = 0;
int gyroHighy = 0;
int gyroLowy = 0;

35
int gyroZerox = 0;
int gyroHighx = 0;
int gyroLowx = 0;
void setup(){
Wire.begin();
Serial.begin(9600);
// Serial.println("starting up L3G4200D");
setupL3G4200D(250); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(2000); //wait for the sensor to be ready
calibrate();
}
//current_rotational_rate[degrees/s]=sensor_value[digits]*8.25[millidegrees/s/digit]*(1/1000)[degree/
millidegree]
//current_angle[deg]=last_angle[deg]+{(current_rotational_rate)[deg/s]*(time_current-time_last)[s]}
//current_angle [deg] = last_angle [deg] + { ((last_rotational_rate+current_rotational_rate)/2) [deg/s] *
(time_current - time_last) [s] }
//
unsigned long pastMicros = 0;
unsigned long currMicros = 0;
float dt = 0.0;

36
void loop()
{
pastMillis = millis();
getGyroValues(); // This will update x, y, and z with new values
pastMicros = currMicros;
currMicros = micros();
if(z >= gyroHighz || z <= gyroLowz)
{
// determine time interval between sensor readings with a bit more accuracy than available just using
millis()
// of course, micros() only updates in intervals of 4 [us], but still far better resolution/sig-figs than millis()
if (currMicros>pastMicros) //micros() overflows/resets every ~70 minutes
dt = (float) (currMicros-pastMicros)/1000000.0;
else
dt = (float) ((4294967295-pastMicros)+currMicros)/1000000.0;
degreesPerSecondz = (((float)z * 8.75))/1000;
// here we do an averaged/smoothed integration of rotational rate to produce current angle.
currentAnglez += ((p_z + degreesPerSecondz)/2) * dt;

37
// Should p_z be updated if z is insufficient to change the current angle? Again, seriously asking because
I trust my mind even less than usual.
p_z = degreesPerSecondz;
}
Serial.print(currentAnglez);
Serial.print("tt");
if(y >= gyroHighy || y <= gyroLowy)
{
millis()
else
degreesPerSecondy = (((float)y * 8.75))/1000;
currentAngley += ((p_y + degreesPerSecondy)/2) * dt;
// Should p_y be updated if y is insufficient to change the current angle? Again, seriously asking because
p_y = degreesPerSecondy;
}

38
Serial.print(currentAngley);
Serial.print("tt");
if(x >= gyroHighx || x <= gyroLowx)
{
millis()
else
degreesPerSecondx = (((float)x * 8.75))/1000;
currentAnglex += ((p_x + degreesPerSecondx)/2) * dt;
// Should p_x be updated if x is insufficient to change the current angle? Again, seriously asking because
p_x = degreesPerSecondx;
}
Serial.println(currentAnglex);

39
// wait ~10 milliseconds
while ( (millis()-pastMillis)<10 ) // millis() overflows once every ~50 days, so less inclined to worry
{
delayMicroseconds(50);
}
}
void calibrate()
{
for(int i = 0; i < 500; i++)
{
getGyroValues();
calibrationSumz += z;
if(z > gyroHighz)
{
gyroHighz = z;
}
else if(z < gyroLowz)
{
gyroLowz = z;
}
gyroZeroz = calibrationSumz / 100;
calibrationSumy += y;

40
if(y > gyroHighy)
{
gyroHighy = y;
}
else if(y < gyroLowy)
{
gyroLowy = y;
}
gyroZeroy = calibrationSumy / 100;
calibrationSumx += x;
if(x > gyroHighx)
{
gyroHighx = x;
}
else if(x < gyroLowx)
{
gyroLowx = x;
}
gyroZerox = calibrationSumx / 100;
}
}

41
void getGyroValues(){
byte xMSB = readRegister(L3G4200D_Address, 0x29);
byte xLSB = readRegister(L3G4200D_Address, 0x28);
x = ((xMSB << 8) | xLSB);
byte yMSB = readRegister(L3G4200D_Address, 0x2B);
byte yLSB = readRegister(L3G4200D_Address, 0x2A);
y = ((yMSB << 8) | yLSB);
byte zMSB = readRegister(L3G4200D_Address, 0x2D);
byte zLSB = readRegister(L3G4200D_Address, 0x2C);
z = ((zMSB << 8) | zLSB);
}
int setupL3G4200D(int scale){
//From Jim Lindblom of Sparkfun's code
// Enable x, y, z and turn off power down:
writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
// Configure CTRL_REG3 to generate data ready interrupt on INT2

42
// No interrupts used on INT1, if you'd like to configure INT1
// or INT2 otherwise, consult the datasheet:
// CTRL_REG4 controls the full-scale range, among other things:
if(scale == 250){
}else if(scale == 500){
}else{
}
// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you'd like:
}
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
}

43
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1); // read a byte
while(!Wire.available()) {
// waiting
}
v = Wire.read();
return v;
}

44
APPENDIX B
Magnetometer (tilt compensated)
//code by Arne
//the acclerometer it set up the way that the z acceleration looks
//to the sky, and x y is flat --> like most cartesian coordinate systems
//I use an A7270 as an Accelerometer -> they are on a breakout board for
approx $20 USD
//and a HMC5843 as a triple axis magnetic Sensor $50 USD
//feel free to comment on the code -> improvement is always appreciated
//you can ask questions in English or German
//required for calculations
#include <math.h>
//required for HMC5843 communication
#include <HMC.h>
//accelerometer connecteded to pins 0 through 2
#define xAxis 0
#define yAxis 1
#define zAxis 2
//by increasing alphaAccel the response will become faster
//but the noise will increae [alpha must be between 0 and 1]
//values for digital lowpass
float alphaAccel = 0.4;
float alphaMagnet = 0.4;
unsigned int xOffset=0;
unsigned int yOffset=0;
unsigned int zOffset=0;
float Pitch=0;
float Roll=0;
float Yaw=0;
int xRaw=0;
int yRaw=0;
int zRaw=0;
float xFiltered=0;
float yFiltered=0;
float zFiltered=0;
float xFilteredOld=0;
float yFilteredOld=0;
float zFilteredOld=0;

45
float xAccel=0;
float yAccel=0;
float zAccel=0;
int xMagnetRaw=0;
int yMagnetRaw=0;
int zMagnetRaw=0;
float xMagnetFiltered=0;
float yMagnetFiltered=0;
float zMagnetFiltered=0;
float xMagnetFilteredOld=0;
float yMagnetFilteredOld=0;
float zMagnetFilteredOld=0;
int xMagnetMax=0;
int yMagnetMax=0;
int zMagnetMax=0;
int xMagnetMin=10000;
int yMagnetMin=10000;
int zMagnetMin=10000;
float xMagnetMap=0;
float yMagnetMap=0;
float zMagnetMap=0;
float YawU;
//initialize µController
void setup()
{
Serial.begin(115200); //initialize serial port
analogReference(EXTERNAL); //use external reference voltage (3,3V)
delay(2000); //calibrate sensor after a short delay
HMC.init();
getAccelOffset(); //keep it flat and non moving on the table
//there are other ways to calibrate the offset, each has some advantes
//and disadvantes..
//compare application note AN3447
//http://www.freescale.com/files/sensors/doc/app_note/AN3447.pdf
}
void FilterAD()
{
// read from AD and subtract the offset
xRaw=analogRead(xAxis)-xOffset;
yRaw=analogRead(yAxis)-yOffset;
zRaw=analogRead(zAxis)-zOffset;
//Digital Low Pass - compare: (for accelerometer)
//http://en.wikipedia.org/wiki/Low-pass_filter
xFiltered= xFilteredOld + alphaAccel * (xRaw - xFilteredOld);

46
yFiltered= yFilteredOld + alphaAccel * (yRaw - yFilteredOld);
zFiltered= zFilteredOld + alphaAccel * (zRaw - zFilteredOld);
xFilteredOld = xFiltered;
yFilteredOld = yFiltered;
zFilteredOld = zFiltered;
//read from Compass
//Digital Low Pass for Noise Reduction for Magnetic Sensor
HMC.getValues(&xMagnetRaw,&yMagnetRaw,&zMagnetRaw);
xMagnetFiltered= xMagnetFilteredOld + alphaMagnet * (xMagnetRaw -
xMagnetFilteredOld);
yMagnetFiltered= yMagnetFilteredOld + alphaMagnet * (yMagnetRaw -
yMagnetFilteredOld);
zMagnetFiltered= zMagnetFilteredOld + alphaMagnet * (zMagnetRaw -
zMagnetFilteredOld);
xMagnetFilteredOld = xMagnetFiltered;
yMagnetFilteredOld = yMagnetFiltered;
zMagnetFilteredOld = zMagnetFiltered;
}
void AD2Degree()
{
// 3.3 = Vref; 1023 = 10Bit AD; 0.8 = Sensivity Accelerometer
// (compare datasheet of your accelerometer)
// the *Accel must be between -1 and 1; you may have to
// to add/subtract +1 depending on the orientation of the accelerometer
// (like me on the zAccel)
// they are not necessary, but are useful for debugging
xAccel=xFiltered *3.3 / (1023.0*0.8);
yAccel=yFiltered *3.3 / (1023.0*0.8);
zAccel=zFiltered *3.3 / (1023.0*0.8)+1.0;
// Calculate Pitch and Roll (compare Application Note AN3461 from Freescale
// http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
// Microsoft Excel switches the values for atan2
// -> this info can make your life easier :-D
//angled are radian, for degree (* 180/3.14159)
Roll = atan2( yAccel , sqrt(sq(xAccel)+sq(zAccel)));
Pitch = atan2( xAccel , sqrt(sq(yAccel)+sq(zAccel)));
}
void getAzimuth()
{
//this part is required to normalize the magnetic vector
if (xMagnetFiltered>xMagnetMax) {xMagnetMax = xMagnetFiltered;}
if (yMagnetFiltered>yMagnetMax) {yMagnetMax = yMagnetFiltered;}
if (zMagnetFiltered>zMagnetMax) {zMagnetMax = zMagnetFiltered;}
if (xMagnetFiltered<xMagnetMin) {xMagnetMin = xMagnetFiltered;}

47
if (yMagnetFiltered<yMagnetMin) {yMagnetMin = yMagnetFiltered;}
if (zMagnetFiltered<zMagnetMin) {zMagnetMin = zMagnetFiltered;}
float norm;
xMagnetMap = float(map(xMagnetFiltered, xMagnetMin, xMagnetMax, -10000,
10000))/10000.0;
yMagnetMap = float(map(yMagnetFiltered, yMagnetMin, yMagnetMax, -10000,
10000))/10000.0;
zMagnetMap = float(map(zMagnetFiltered, zMagnetMin, zMagnetMax, -10000,
10000))/10000.0;
//normalize the magnetic vector
norm= sqrt( sq(xMagnetMap) + sq(yMagnetMap) + sq(zMagnetMap));
xMagnetMap /=norm;
yMagnetMap /=norm;
zMagnetMap /=norm;
//compare Applications of Magnetic Sensors for Low Cost Compass Systems by
Michael J. Caruso
//for the compensated Yaw equations...
//http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
Yaw=atan2( (-yMagnetMap*cos(Roll) + zMagnetMap*sin(Roll) ) ,
xMagnetMap*cos(Pitch) + zMagnetMap*sin(Pitch)*sin(Roll)+
zMagnetMap*sin(Pitch)*cos(Roll)) *180/PI;
YawU=atan2(-yMagnetMap, xMagnetMap) *180/PI;
}
void loop()
{
FilterAD();
AD2Degree();
getAzimuth();
Send2Com();
}
void Send2Com()
{
Serial.print("Pitch: ");
Serial.print(int(Pitch*180/PI));
Serial.print(" Roll: ");
Serial.print(int(Roll*180/PI));
Serial.print(" Yaw: ");
Serial.print(int(Yaw));
Serial.print(" YawU: ");
Serial.println(int(YawU));
delay(50);
}
void getAccelOffset()

48
{ //you can make approx 60 iterations because we use an unsigned int
//otherwise you get an overflow. But 60 iterations should be fine
for (int i=1; i <= 60; i++){
xOffset += analogRead(xAxis);
yOffset += analogRead(yAxis);
zOffset += analogRead(zAxis);
}
xOffset /=60;
yOffset /=60;
zOffset /=60;
Serial.print("xOffset: ");
Serial.print(xOffset);
Serial.print(" yOffset: ");
Serial.print(yOffset);
Serial.print(" zOffset: ");
Serial.println(zOffset);
}
Sun sensor coding
%%serial communication of measurements from celestial, magneto, accelero-
gyro
s = serial('COM1','BaudRate',115200);
fopen(s);
s.status
%% Celestial Calculations
% Calculation of Si
x = 0;
zzzz=1
while(zzzz<50)
c=clock
d=fix(c);
yo=d(1);
mo=d(2);
da=d(3);
h=d(4);
mi=d(5);
se=d(6);
%Julian Date

49
jd=juliandate(yo,mo,da,h,mi,se);
TU=((jd-2451545)/36525);
LAMDAM=((280.4606184*pi/180)+36000.77005361*TU);
TB=TU;
Msun=((357.5277233*pi/180)+35999.05034*TB);
LAMDAE=LAMDAM+(1.914666471*pi/180)*sin(Msun)+0.918994643*sin(2*Msun)
sesteric=(1.000140612-.016708617*cos(Msun)-.000139589*cos(2*Msun));
epsilon=((23.439291*pi/180)-0.0130042*TB);
Si=[cos(LAMDAE);cos(epsilon*sin(LAMDAE));sin(epsilon*sin(LAMDAE))]
%Calculation of Sb
%I0=max value get from hardware
I0=0.5*255/5;
%a0=angle between nomal and photocell from hardware
a0=pi/4;
lcm=fscanf(s)
% for ii=1:10000
%
% end
current=str2num(lcm)
current1=current(4);
delI1=current1;
C1=delI1/(2*I0*sin(a0))
a1=asin(C1)
delI2=current2;
a2=asin(C2)
%SS=Ss*
SS=[1; tan(a1)/tan(a2); tan(a1)]
Ss=SS/((transpose(SS)*SS)^0.5)
th1=0;
th2=0;
th3=0;
R31=[cos(th1) sin(th1) 0;-sin(th1) cos(th1) 0;0 0 1];
R22=[cos(th2) 0 -sin(th2);0 1 0;sin(th2) 0 cos(th2)];
R13=[1 0 0;0 cos(th3) sin(th3);0 -sin(th3) cos(th3)];
Rbs=R31*R22*R13

50
Sb=Rbs*Ss;
%Calculation of Rbi
% from sun measured values
v1b=Sb;
% from magnetometer measured values
mB=[current(1);current(2);current(3)];
mb=mB/sqrt(transpose(mB)*mB);
% for ii=1:10000
%
% end
v2b=mb;
% sun known inertial frame components
v1i=Si;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (mo==4)
acd=10+da;
end
if (mo==5)
acd=40+da;
end
if (mo==6)
acd=710+da;
end
if (mo==7)
acd=101+da;
end
if (mo==8)
acd=132+da;
end
if (mo==9)
acd=163+da;
end
if (mo==10)
acd=193+da;
end
if (mo==11)
acd=224+da;
end
if (mo==12)
acd=254+da;
end
if (mo==1)
acd=285+da;

51
end
if (mo==2)
acd=316+da;
end
if (mo==3)
if(da<21)
acd=344+da;
else
acd=da-21;
end
end
%Greenwichmarinetime in radians
gw=(235.9*acd*pi)/43100;
if(h>17)
h=h-17;
end
if(h<17)
h=h+17;
end
t=h*3600+mi*60+se;
alpham=gw+((2*pi/86200)*t)+108.43*pi/180;
thetam=196.54*pi/180;
di=[sin(thetam)*cos(alpham);
sin(thetam)*sin(alpham);
cos(thetam)];
H=0.000030115;
thetalo=55.9*pi/180;
philo=72.7*pi/180+gw;
ri=[cos(thetalo)*cos(philo); cos(thetalo)*sin(philo); sin(thetalo)];
rat = transpose(ri) %new change%
%mI=mi*
mI=H*[3*dot(di,ri)*ri(1)-di(1);
3*dot(di,ri)*ri(2)-di(2);
3*dot(di,ri)*ri(3)-di(3)]
mi=mI/(sqrt(transpose(mI)*mI));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%
v2i = mi;
t1b=v1b;
yb=cross(v1b,v2b);

52
xb=(yb(1)^2+yb(2)^2+yb(3)^2)^0.5;
t2b=yb/xb;
t3b=cross(t1b,t2b);
t1i=v1i;
yi=cross(v1i,v2i);
xi=(yi(1)^2+yi(2)^2+yi(3)^2)^0.5;
t2i=yi/xi;
t3i=cross(t1i,t2i);
Rbt=[t1b t2b t3b];
Rit=[t1i t2i t3i];
Rti=transpose(Rit);
Rbicm=Rbt*Rti;
% convertion to quateranian of rotation matrix Rbi
% q4cm=0.5*sqrt(1+trace(Rbicm));
%
% qcm=(0.25/q4cm)*[Rbicm(2,3)-Rbicm(3,2);Rbicm(3,1)-Rbicm(1,3);Rbicm(1,2)-
Rbicm(2,1)];
%
% q1cm=[transpose(qcm) q4cm] %qicm%
%
% phicm=2*acos(q4cm);
%
% acm=qcm*asin(phicm/2);
phicm=acos(0.5*(trace(Rbicm)-1));
axcm=(1/(2*sin(phicm)))*(transpose(Rbicm)-Rbicm);
acm=[axcm(3,2); axcm(1,3); axcm(2,1)];
qcm=acm*sin(phicm/2);
q4cm=cos(phicm/2);
q1cm=[transpose(qcm) q4cm]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y1=[current(6) current(7) current(8)];
R31=[cos(y1(1)) sin(y1(1)) 0;-sin(y1(1)) cos(y1(1)) 0;0 0 1];
R22=[cos(y1(2)) 0 -sin(y1(2));0 1 0; sin(y1(2)) 0 cos(y1(2))];
R13=[1 0 0;0 cos(y1(3)) sin(y1(3));0 -sin(y1(3)) cos(y1(3))];
Rbigy=R31*R22*R13;
phigy=acos(0.5*(trace(Rbigy)-1));
axgy=(1/(2*sin(phigy)))*(transpose(Rbigy)-Rbigy);
agy=[axgy(3,2); axgy(1,3); axgy(2,1)];
qgy=agy*sin(phigy/2);
q4gy=cos(phigy/2);
q1gy=[transpose(qgy) q4gy]

53
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Kalman routine
if (x == 0)
%initial kalman gain
k = 1/2;
l = 1/2;
%initial variance
% var_m1 = [2.3;0.4;1.1]; %input('Enter variance specified for sensor
magneto');
% var_m2 = [0.3;0.4;1.1;6] %input('Enter variance specified for sensor
celestial');
% belo = [0.012;0.1;8] %input('Enter variance specified for sensor gyro');
%
%
%
var_m1 = [2.197458 0 0 0;0 1.885593 0 0;0 0 1.988701 0;0 0 0
1.89756]%magneto variance
var_m2=[0.31 0 0 0;0 0.32 0 0;0 0 0.33 0;0 0 0 0.34]%celestial variance
belo =[0.669661 0 0 0;0 0.323559 0 0;0 0 0.410976 0;0 0 0 -0.527221]%gyro
variance
elo = sqrt(var_m1.^2+var_m2.^2);%combined variance celestial and megneto
phy_variance=elo;
%initializing
pin1=[0 0 1]*transpose(rat);
pin = [rat cos(55.9*(pi/180))];
result = [transpose(pin)]; %physical_n-1_state
predicted =result; % predicted value for first kalman
predict = result; % predicted value for second kalman
%initial error coveriance for kalman
P_k=[0.001 0 0 0;
0 0.001 0 0;
0 0 0.001 0;
0 0 0 0.001];
%initial error coveriance for kalmanextended26
P_kk=P_k;
x = x + 1;
end
%measurement_(celestial+magneto)_s1+s2
a = transpose(q1cm);
%measurement_(gyro-accelero)_s3
b = transpose(q1gy);

54
% kalman filter
[act,corvar] = kalman(predicted,a,b,elo,belo,P_k)
P_k=corvar;
predicted=act;
%measurement_nth_physical_n-1_result
dope = result;
% kalman filter2
[result,corvar1] =
kalmanextended26(predict,predicted,dope,corvar,phy_variance,P_kk)
P_kk=corvar1;
predict=result;
result
zzzz=zzzz+1;
end
%% Kalman Routine Ends
fclose(s);
s.status
Kalman Filter
%%serial communication of measurements from celestial, magneto, accelero-
gyro
s = serial('COM1','BaudRate',115200);
fopen(s);
s.status
%% Celestial Calculations
% Calculation of Si
x = 0;
zzzz=1
while(zzzz<50)

55
c=clock
d=fix(c);
yo=d(1);
mo=d(2);
da=d(3);
h=d(4);
mi=d(5);
se=d(6);
%Julian Date
jd=juliandate(yo,mo,da,h,mi,se);
TU=((jd-2451545)/36525);
LAMDAM=((280.4606184*pi/180)+36000.77005361*TU);
TB=TU;
Msun=((357.5277233*pi/180)+35999.05034*TB);
LAMDAE=LAMDAM+(1.914666471*pi/180)*sin(Msun)+0.918994643*sin(2*Msun)
sesteric=(1.000140612-.016708617*cos(Msun)-.000139589*cos(2*Msun));
epsilon=((23.439291*pi/180)-0.0130042*TB);
Si=[cos(LAMDAE);cos(epsilon*sin(LAMDAE));sin(epsilon*sin(LAMDAE))]
%Calculation of Sb
%I0=max value get from hardware
I0=0.5*255/5;
%a0=angle between nomal and photocell from hardware
a0=pi/4;
lcm=fscanf(s)
% for ii=1:10000
%
% end
current=str2num(lcm)
delI1=current1;
a1=asin(C1)
delI2=current2;
a2=asin(C2)
%SS=Ss*
SS=[1; tan(a1)/tan(a2); tan(a1)]
Ss=SS/((transpose(SS)*SS)^0.5)

56
th1=0;
th2=0;
th3=0;
R31=[cos(th1) sin(th1) 0;-sin(th1) cos(th1) 0;0 0 1];
R22=[cos(th2) 0 -sin(th2);0 1 0;sin(th2) 0 cos(th2)];
R13=[1 0 0;0 cos(th3) sin(th3);0 -sin(th3) cos(th3)];
Rbs=R31*R22*R13
Sb=Rbs*Ss;
%Calculation of Rbi
% from sun measured values
v1b=Sb;
% from magnetometer measured values
mB=[current(1);current(2);current(3)];
mb=mB/sqrt(transpose(mB)*mB);
% for ii=1:10000
%
% end
v2b=mb;
% sun known inertial frame components
v1i=Si;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (mo==4)
acd=10+da;
end
if (mo==5)
acd=40+da;
end
if (mo==6)
acd=710+da;
end
if (mo==7)
acd=101+da;
end
if (mo==8)
acd=132+da;
end
if (mo==9)
acd=163+da;

57
end
if (mo==10)
acd=193+da;
end
if (mo==11)
acd=224+da;
end
if (mo==12)
acd=254+da;
end
if (mo==1)
acd=285+da;
end
if (mo==2)
acd=316+da;
end
if (mo==3)
if(da<21)
acd=344+da;
else
acd=da-21;
end
end
%Greenwichmarinetime in radians
gw=(235.9*acd*pi)/43100;
if(h>17)
h=h-17;
end
if(h<17)
h=h+17;
end
t=h*3600+mi*60+se;
alpham=gw+((2*pi/86200)*t)+108.43*pi/180;
thetam=196.54*pi/180;
di=[sin(thetam)*cos(alpham);
sin(thetam)*sin(alpham);
cos(thetam)];
H=0.000030115;
thetalo=55.9*pi/180;
philo=72.7*pi/180+gw;
ri=[cos(thetalo)*cos(philo); cos(thetalo)*sin(philo); sin(thetalo)];
rat = transpose(ri) %new change%
%mI=mi*

58
mI=H*[3*dot(di,ri)*ri(1)-di(1);
3*dot(di,ri)*ri(2)-di(2);
3*dot(di,ri)*ri(3)-di(3)]
mi=mI/(sqrt(transpose(mI)*mI));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%
v2i = mi;
t1b=v1b;
yb=cross(v1b,v2b);
xb=(yb(1)^2+yb(2)^2+yb(3)^2)^0.5;
t2b=yb/xb;
t3b=cross(t1b,t2b);
t1i=v1i;
yi=cross(v1i,v2i);
xi=(yi(1)^2+yi(2)^2+yi(3)^2)^0.5;
t2i=yi/xi;
t3i=cross(t1i,t2i);
Rbt=[t1b t2b t3b];
Rit=[t1i t2i t3i];
Rti=transpose(Rit);
Rbicm=Rbt*Rti;
% convertion to quateranian of rotation matrix Rbi
% q4cm=0.5*sqrt(1+trace(Rbicm));
%
% qcm=(0.25/q4cm)*[Rbicm(2,3)-Rbicm(3,2);Rbicm(3,1)-Rbicm(1,3);Rbicm(1,2)-
Rbicm(2,1)];
%
% q1cm=[transpose(qcm) q4cm] %qicm%
%
% phicm=2*acos(q4cm);
%
% acm=qcm*asin(phicm/2);
phicm=acos(0.5*(trace(Rbicm)-1));
axcm=(1/(2*sin(phicm)))*(transpose(Rbicm)-Rbicm);
acm=[axcm(3,2); axcm(1,3); axcm(2,1)];
qcm=acm*sin(phicm/2);
q4cm=cos(phicm/2);
q1cm=[transpose(qcm) q4cm]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y1=[current(6) current(7) current(8)];

59
R31=[cos(y1(1)) sin(y1(1)) 0;-sin(y1(1)) cos(y1(1)) 0;0 0 1];
R22=[cos(y1(2)) 0 -sin(y1(2));0 1 0; sin(y1(2)) 0 cos(y1(2))];
R13=[1 0 0;0 cos(y1(3)) sin(y1(3));0 -sin(y1(3)) cos(y1(3))];
Rbigy=R31*R22*R13;
phigy=acos(0.5*(trace(Rbigy)-1));
axgy=(1/(2*sin(phigy)))*(transpose(Rbigy)-Rbigy);
agy=[axgy(3,2); axgy(1,3); axgy(2,1)];
qgy=agy*sin(phigy/2);
q4gy=cos(phigy/2);
q1gy=[transpose(qgy) q4gy]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Kalman routine
if (x == 0)
%initial kalman gain
k = 1/2;
l = 1/2;
%initial variance
% var_m1 = [2.3;0.4;1.1]; %input('Enter variance specified for sensor
magneto');
% var_m2 = [0.3;0.4;1.1;6] %input('Enter variance specified for sensor
celestial');
% belo = [0.012;0.1;8] %input('Enter variance specified for sensor gyro');
%
%
%
var_m1 = [2.197458 0 0 0;0 1.885593 0 0;0 0 1.988701 0;0 0 0
1.89756]%magneto variance
var_m2=[0.31 0 0 0;0 0.32 0 0;0 0 0.33 0;0 0 0 0.34]%celestial variance
belo =[0.669661 0 0 0;0 0.323559 0 0;0 0 0.410976 0;0 0 0 -0.527221]%gyro
variance
elo = sqrt(var_m1.^2+var_m2.^2);%combined variance celestial and megneto
phy_variance=elo;
%initializing
pin1=[0 0 1]*transpose(rat);
pin = [rat cos(55.9*(pi/180))];
result = [transpose(pin)]; %physical_n-1_state
predicted =result; % predicted value for first kalman
predict = result; % predicted value for second kalman
%initial error coveriance for kalman
P_k=[0.001 0 0 0;
0 0.001 0 0;
0 0 0.001 0;
0 0 0 0.001];

60
%initial error coveriance for kalmanextended26
P_kk=P_k;
x = x + 1;
end
%measurement_(celestial+magneto)_s1+s2
a = transpose(q1cm);
%measurement_(gyro-accelero)_s3
b = transpose(q1gy);
% kalman filter
[act,corvar] = kalman(predicted,a,b,elo,belo,P_k)
P_k=corvar;
predicted=act;
%measurement_nth_physical_n-1_result
dope = result;
% kalman filter2
[result,corvar1] =
P_kk=corvar1;
predict=result;
result
zzzz=zzzz+1;
end
%% Kalman Routine Ends
fclose(s);
s.status
% function [ypo,kpo] =
kalmanextended26(predict,predicted,dope,corvar,phy_variance)
function [result,corvar1] =
xhat_kk=predict;
z_kk=predicted;
zzhat=dope;

61
AA=eye(4);
HH=AA;
P_kk=AA*P_kk*transpose(AA)+corvar;
%compute the kalman gain
kp = (P_kk*transpose(HH))/(HH*P_kk*transpose(HH)+phy_variance);
%update estimate with measurement z_k
xhat_kk=xhat_kk+kp*(z_kk-zzhat);
%update the error covariance
P_kk=((eye(length(xhat_kk)))-kp*HH)*P_kk;
%updating predicted error variance
result=xhat_kk;
corvar1=P_kk;

Dsp kit implementation of Kalman filter based sensor fused algorithm for attitude sensors

More Related Content

What's hot (19)

Similar to Dsp kit implementation of Kalman filter based sensor fused algorithm for attitude sensors (20)

More from Zorays Solar Pakistan (19)

Recently uploaded (20)

Dsp kit implementation of Kalman filter based sensor fused algorithm for attitude sensors