## Just for you: FREE 60-day trial to the world’s largest digital library.

The SlideShare family just got bigger. Enjoy access to millions of ebooks, audiobooks, magazines, and more from Scribd.

Cancel anytime.Free with a 14 day trial from Scribd

- 1. 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. 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. 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. 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. 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. 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. 7 So our project has following major parts Controls Algorithm development Integration and interfacing Figure 1.1- Block Diagram
- 8. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 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. 33 Wire.beginTransmission(DEVICE); // start transmission to device Wire.write(address); // sends address to read from Wire.endTransmission(); // end transmission Wire.beginTransmission(DEVICE); // start transmission to device 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++; } Wire.endTransmission(); // end transmission } Gyroscope //Arduino 1.0+ only #include <Wire.h> #define CTRL_REG1 0x20 #define CTRL_REG2 0x21 #define CTRL_REG3 0x22 #define CTRL_REG4 0x23 #define CTRL_REG5 0x24 int L3G4200D_Address = 105; //I2C address of the L3G4200D
- 34. 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. 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. 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. 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) { // 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; degreesPerSecondy = (((float)y * 8.75))/1000; // here we do an averaged/smoothed integration of rotational rate to produce current angle. currentAngley += ((p_y + degreesPerSecondy)/2) * dt; // Should p_y be updated if y is insufficient to change the current angle? Again, seriously asking because I trust my mind even less than usual. p_y = degreesPerSecondy; }
- 38. 38 Serial.print(currentAngley); Serial.print("tt"); if(x >= gyroHighx || x <= gyroLowx) { // 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; degreesPerSecondx = (((float)x * 8.75))/1000; // here we do an averaged/smoothed integration of rotational rate to produce current angle. currentAnglex += ((p_x + degreesPerSecondx)/2) * dt; // Should p_x be updated if x is insufficient to change the current angle? Again, seriously asking because I trust my mind even less than usual. p_x = degreesPerSecondx; } Serial.println(currentAnglex);
- 39. 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. 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. 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: writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000); // Configure CTRL_REG3 to generate data ready interrupt on INT2
- 42. 42 // No interrupts used on INT1, if you'd like to configure INT1 // or INT2 otherwise, consult the datasheet: writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000); // CTRL_REG4 controls the full-scale range, among other things: if(scale == 250){ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000); }else if(scale == 500){ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000); }else{ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000); } // CTRL_REG5 controls high-pass filtering of outputs, use it // if you'd like: writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000); } 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 Wire.endTransmission(); // end transmission }
- 43. 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. 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. 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. 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. 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. 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. 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); current2=current(5); delI1=current1; C1=delI1/(2*I0*sin(a0)) a1=asin(C1) delI2=current2; C2=delI2/(2*I0*sin(a0)) 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. 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. 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. 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. 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. 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. 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) current1=current(4); current2=current(5); delI1=current1; C1=delI1/(2*I0*sin(a0)) a1=asin(C1) delI2=current2; C2=delI2/(2*I0*sin(a0)) a2=asin(C2) %SS=Ss* SS=[1; tan(a1)/tan(a2); tan(a1)] Ss=SS/((transpose(SS)*SS)^0.5)
- 56. 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. 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. 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. 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. 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] = 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 % function [ypo,kpo] = kalmanextended26(predict,predicted,dope,corvar,phy_variance) function [result,corvar1] = kalmanextended26(predict,predicted,dope,corvar,phy_variance,P_kk) xhat_kk=predict; z_kk=predicted; zzhat=dope;
- 61. 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;