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.

http://en.wikipedia.org/wiki/Piezoresistive_effect

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.

http://en.wikipedia.org/wiki/G-force

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 ŝ =siTi

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

http://en.wikipedia.org/wiki/GPS

http://en.wikipedia.org/wiki/Dead_reckoning

http://en.wikipedia.org/wiki/Drift_%28telecommunication%29

http://en.wikipedia.org/wiki/Newton%27s_laws_of_motion

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

http://en.wikipedia.org/wiki/Accelerometer

http://en.wikipedia.org/wiki/Gyroscope

http://en.wikipedia.org/wiki/magnetometer

http://www.arduino.cc/

http://www.pololu.com/file/download/L3G4200D.pdf

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("\t\t");

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("\t\t");

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

Technology

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