Usman

FINDING OPTIMAL ROTATION AND TRANSLATION BETWEEN CORRESPONDING 3D POINTS

2016-09-03T18:39:00.001+10:00

Finding the optimal/best rotation and translation between two sets of corresponding 3D point data, so that they are aligned/registered, is a common problem I come across. An illustration of the problem is shown below for the simplest case of 3 corresponding points (the minimum required points to solve).

The corresponding points have the same colour, R is the rotation and t is the translation. We want to find the best rotation and translation that will align the points in dataset A to dataset B. Here, ‘optimal’ or ‘best’ is in terms of least square errors. This transformation is sometimes called the Euclidean or Rigid transform, because it preserves the shape and size. This is in contrast to an affine transform, which includes scaling and shearing.

This problem arises especially in tasks like 3D point cloud data registration, where the data is obtained from hardware like a 3D laser scanner or the popular Kinect device. The solution I’ll be presenting is from the paper ‘A Method for Registration of 3-D Shapes’, by Besl and McKay, 1992.

Solution overview

We’re solving for R,t in the equation:

B = R*A + t

Where R,t are the transforms applied to dataset A to align it with dataset B, as best as possible.

Finding the optimal rigid transformation matrix can be broken down into the following steps:

Find the centroids of both dataset
Bring both dataset to the origin then find the optimal rotation, (matrix R)
Find the translation t

Finding the centroids

This bit is easy, the centroids are just the average point and can be calculated as follows:

Here, and are points in dataset A and B respectively. We will use these values in the next step.

Finding the optimal rotation

There are a few ways of finding optimal rotations between points. The easiest way I found is using Singular Value Decomposition (SVD), because it’s a function that is widely available in many programming languages (Matlab, Octave, C using LAPACK, C++ using OpenCV …). SVD is like this powerful magical wand in linear algebra for solving all sorts of numerical problems, but tragically wasn’t taught when I was at uni. I won’t go into details on how it works but rather how to use it. You only need to know that the SVD will decompose/factorise a matrix (call it E), into 3 other matrices, such that:

If E is a square matrix then U, S and V are the same size as well. We’re only interested in square matrices for this problem so I won’t go into detail about rectangular ones.

To find the optimal rotation we first re-centre both dataset so that both centroids are at the origin, like shown below.

This removes the translation component, leaving on the rotation to deal with. The next step involves accumulating a matrix, called H, and using SVD to find the rotation as follows:

H is the familiar covariance matrix.

At this point you may be thinking, “what the, that easy?!”, and indeed you would be right. One thing to be careful is that you calculate Hcorrectly. It should end up being a 3×3 matrix, not a 1×1 matrix. Pay close attention to the transpose symbol. It’s doing a multiplication between 2 matrices where the dimensions effectively are, 3×1 and 1×3, respectively. The ordering of the multiplication is also important, doing it the other way will find a rotation from B to A instead.

Special reflection case

There’s a special case when finding the rotation matrix that you have to take care of. Sometimes the SVD will return a ‘reflection’ matrix, which is numerically correct but is actually nonsense in real life. This is addressed by checking the determinant of V (from SVD above) and seeing if it’s negative (-1). If it is then the 3rd column of V is multiplied by -1.

if determinant(R) &lt; 0
    multiply 3rd column of V by -1
    recompute R
end if

An alternative check that is possibly more robust was suggested by Nick Lambert

if determinant(R) &lt; 0
    multiply 3rd column of R by -1
end if

where R is the rotation matrix.

A big thank you goes to Klass Jan Russcher and Nick Lambert for these solutions.

Finding t

The translation is

The centroids are 3×1 column vectors.

How did I get this? If you remember back, to transform A to B we first had to centre A to its origin. This is where the -centroid_A comes from, though I put the minus sign at the front. We then rotate A, hence R. Then finally translate it to dataset B’s origin, the + centroid_B bit.

And we’re done!

Note on usage

The solution presented can be used on any size dataset as long as there are at least 3 points. When there are more than 3 points a least square solution is obtained, such that the following error is minimised:

Here, the operator || || is the Euclidean distance between two vectors, a scalar value. err is just the square distance error between the points in the two dataset.

source(Great tutorial from :::http://nghiaho.com/?page_id=671)

Carina 2 "Self Driving Car"

2015-04-12T03:01:00.002+10:00

I have been following AV related projects and one of them is from Brazil "Carina 2 Self driving".

Demo in News:
(Self driving Toyota Carina and Volvo with smart-auto-cruise ) https://www.youtube.com/watch?v=udk9NdQoVFE

The Toyota Carina is using the map based localization using high-end GPS+wheel encoder+IMU+velodyne for localization (with precise map they do monte-carlo localization. The map is 2D occupancy of elevations). They have drive-by-wire (actuators for steering control, brake, gas). The obstacle detection is mainly done using velodyne 3D data (I presume elevation thresholding for free-space detection).

The Volvo car uses the camera plus radar for smart auto-cruise feature. Keeping in lane (using vision camera) and avoiding obstacle using radar information for automatic steering and gas/brake control

Localization Technology: Map based localization using Velodyne+GPS+encoder+IMU
Object Detection: Velodyne and also Camera for free-space detection (code link below)

Some resources:
a- They have open source code for road-detection using neural network (free space)
video:https://www.youtube.com/watch?v=FEePJBn3Yos
code: http://www.lrm.icmc.usp.br/web/index.php?n=Site.ProjRoadANN

b- Multiple vehicle simulation network
Video: https://www.youtube.com/watch?v=0Hg2ntnhhEM
Description: http://www.lrm.icmc.usp.br/web/index.php?n=Site.ProjSimul

Insight into Mercedes Autonomous Vehicle "On a memorial Bertha Benz route"

2015-04-12T02:55:00.000+10:00

Recently published [1] article details the Mercedes Autonomous Vehicle. The keypoints from localisation/perception perspective are highlighted here:

1- Localization

Using stereo/backward monocular camera plus high-end IMU/GPS/wheel-encoder to make offline maps.
During the online localisation the initial position was determined by GPS/IMU however vision based two-localization techniques are integrated for precise centimeter localisation
   a- Image feature to image feature based localisation
   b- Lane and curb detection from stereo data and then matching with the LANE/CURB based map (Advance form of RNDF (Route Network Defination File) as nice concept of lanelet was introduced http://www.mrt.kit.edu/software/libLanelet/libLanelet.html)

2- Pedestrian detection

Stereo camera
   a- From Stereo ROI is extracted and then trained classifier (HOG+mixture of expert-SVMachines) is used to classify.
   b- Tracking is performed using EKF

3- Vehicle detection

Stereo & (Radar is used separetly and the results are not fused with vision)
a- From Stereo ROI approach similiar to 2a for distance < 40meter.
b- For 200m they used monocular classifier (viola jones adaboost)

4- Traffic Light

Monocular camera because of wide FOV
a- Train a neural network for classification on the record data
b- Did NN matching on real-stream of video

5- Planning
a- Optimization of the cost function which depends upon velocity, jerk, yaw and collision.
   b- They used SQP approach for minimisation

6- Controller
   a- Lateral controller is similiar to lane keeping controller using Feed forward & backward proportional controller
   b- Longitudnal using model predictive control

7- Offline planning
a- Used the map generated earlier for localization and used free OpenStreetMap editor for defining the planned route.

In short the main sensors used were camera+radar+GPS for localisation/perception.

[1] Making Bertha See, ICCV workshop 2013
http://www.cv-foundation.org/openaccess/content_iccv_workshops_2013/W07/papers/Franke_Making_Bertha_See_2013_ICCV_paper.pdf

Simple Idea to Control a car Remotely (using actuators)

2015-03-09T02:38:00.001+11:00

Guys from LabView "Waterloolabs" have built a control systems using a few motors, potentiometers, a Compact RIO embedded controller, and LabVIEW.

They then set up wi-fi communication so that they can drive the car from an iPhone as well as from a modified Power Wheels truck.

For detailed plans, parts lists and code available for download, check out their website http://www.WaterlooLabs.com

(Project done in back 2009)

Low-cost 3D Lidar

2015-02-28T17:24:00.001+11:00

Low-cost Lidar for Autonomous vehicle technology is a big boost for real-time 3D perception. The video mentions the cost of two 3D Lidar (8 Layers) with Tegra K1 processor to be US $2500.

Range: 300 meter

Advantage: No rotating part

Sensor Manufacturer: http://www.quanergy.com

Google New Lidar Sensor (with Multiple camera)

2015-02-28T16:20:00.000+11:00

Regarding the latest development at google Autonomous Vehicle. From the video, it seems they are using their own Lidar sensor rather than velodyne Puck 16. The interesting thing with rotating lidar is the presence of multiple cameras(seems either they are using depth-color known as RGB-D approach to solve various task like localization, pedestrian/vehicle/obstacle detection etc).

Specs from the New Yorker article
"
At the tech meeting, Levandowski showed the team a video of Google’s newest laser, slated to be installed within the year. It had more than twice the range of previous models—eleven hundred feet instead of two hundred and sixty—and thirty times the resolution. At three hundred feet, it could spot a metal plate less than two inches thick. The laser would be about the size of a coffee mug, he told me, and cost around ten thousand dollars—seventy thousand less than the current model."

Some Useful links:
Google Self-driving car
Velodyne Lidar
History about Google AV

Drive by Wire Technology

2014-05-02T17:12:00.000+10:00

Drive-by-wire is a catch-all term that can refer to a number of electronic systems that take the place of old mechanical controls.

Instead of using cables, hydraulic pressure, and other things that provide the driver with direct, physical control over the speed or direction of a vehicle, drive-by-wire technology uses electronic controls to activate the brakes, control the steering, and operate other systems.

The three main drive-by-wire technologies are referred to as:

Brake-By-Wire
Electronic Throttle Control
Steer-By-Wire

Drive by wire is a vehicle that is not controlled by a direct mechanical means by the driver. The throttle is controlled by an electrical servo controller by a computer based on input from the throttle pedal. There is no direct mechanical linkage from the throttle pedal to the throttle valve in the throttle body

The steering is controlled by a servo controlled by the car's computer based on input from the steering wheel. There is no direct mechanical connection from the steering wheel to the steering box. The brakes are controlled by servos in the braking system controlled by the car's computer based on input from the brake pedal. There is no direct mechanical linkage between the brake pedal and the braking system.

PID position Controller for Arducopter with VICON Measurements!

2013-08-30T09:51:00.001+10:00

We have developed a PID position controller that can communicate with arducopter via 3DR radio. The position controller runs on the ground computer and transmits the commands to hexacopter at 50HZ via mavlink protocol. The mavlink packet used during our test flights is "set_roll_pitch_yaw_thrust_send". We have tested the hovering flight for the developed controller.

The source code for ground control is available here : Code of PID_controller
Please note that: I have used the standard code for arducopter communication via ROS available here:
https://code.google.com/p/roscopter/. Please copy the PID controller code into the roscopter's nodes directory. For running please type: python position_controller_for_vicon_measurements.py (bydefault it will check for USB0 for 3RD radio modem at 57600).

The aim is to provide the basic understanding of the controller (its a hierarchical controller i.e. the position controller uses the stabilization controller of the arducopter) --*please note that the arducopter code is also required to be changed to accommodate the mavlink listening + update of roll/pitch/yaw commands in the stablization mode.

Thanks

Location on Maps (Lat/Lon, UTM)

2013-07-08T02:46:00.000+10:00

Location, Distance, and Direction on Maps

Location on Maps

Most maps allow us to specify the location of points on the Earth's surface using a coordinate system. For a two-dimensional map, this coordinate system can use simple geometric relationships between the perpendicular axes on a grid system to define spatial location. Figure 2b-1illustrates how the location of a point can be defined on a coordinate system.

Figure 2b-1: A grid coordinate system defines the location of points from the distance traveled along two perpendicular axes from some stated origin. In the example above, the two axes are labeled X and Y. The origin is located in the lower left hand corner. Unit distance traveled along each axis from the origin is shown. In this coordinate system, the value associated with the X-axis is given first, following by the value assigned from the Y-axis. The location represented by the star has the coordinates 7 (X-axis), 4 (Y-axis).

Two types of coordinate systems are currently in general use in geography: the geographical coordinate system and the rectangular (also calledCartesian) coordinate system.

Geographical Coordinate System

The geographical coordinate system measures location from only two values, despite the fact that the locations are described for a three-dimensional surface. The two values used to define location are both measured relative to the polar axis of the Earth. The two measures used in the geographic coordinate system are called latitude and longitude.

Figure 2b-2: Lines of latitude or parallels are drawn parallel to the equator (shown in red) as circles that span the Earth's surface. These parallels are measure in degrees (°). There are 90 angular degrees of latitude from the equator to each of the poles. The equator has an assigned value of 0°. Measurements of latitude are also defined as being either north or south of equator to distinguish the hemisphere of their location. Lines of longitude or meridians are circular arcs that meet at the poles. There are 180° of longitude either side of a starting meridian which is known the Prime Meridian. The Prime Meridian has a designated value of 0°. Measurements of longitude are also defined as being either west or east of the Prime Meridian.

Latitude measures the north-south position of locations on the Earth's surface relative to a point found at the center of the Earth (Figure 2b-2). This central point is also located on the Earth's rotational or polar axis. The equator is the starting point for the measurement of latitude. The equator has a value of zero degrees. A line of latitude or parallel of 30° North has an angle that is 30° north of the plane represented by the equator (Figure 2b-3). The maximum value that latitude can attain is either 90° North or South. These lines of latitude run parallel to the rotational axis of the Earth.

Figure 2b-3: Measurement of latitude and longitude relative to the equator and the Prime Meridian and the Earth's rotational or polar axis.

Longitude measures the west-east position of locations on the Earth's surface relative to a circular arc called the Prime Meridian (Figure 2b-2). The position of the Prime Meridian was determined by international agreement to be in-line with the location of the former astronomical observatory at Greenwich, England. Because the Earth's circumference is similar to circle, it was decided to measure longitude in degrees. The number of degrees found in a circle is 360. The Prime Meridian has a value of zero degrees. A line of longitude or meridian of 45° West has an angle that is 45° west of the plane represented by the Prime Meridian (Figure 2b-3). The maximum value that a meridian of longitude can have is 180° which is the distance halfway around a circle. This meridian is called the International Date Line. Designations of west and east are used to distinguish where a location is found relative to the Prime Meridian. For example, all of the locations in North America have a longitude that is designated west.

Universal Transverse Mercator System (UTM)

Another commonly used method to describe location on the Earth is the Universal Transverse Mercator (UTM) grid system. This rectangularcoordinate system is metric, incorporating the meter as its basic unit of measurement. UTM also uses the Transverse Mercator projection system to model the Earth's spherical surface onto a two-dimensional plane. The UTM system divides the world's surface into 60 - six degree longitude wide zones that run north-south (Figure 2b-5). These zones start at the International Date Line and are successively numbered in an eastward direction (Figure 2b-5). Each zone stretches from 84° North to 80° South (Figure 2b-4). In the center of each of these zones is a central meridian. Location is measured in these zones from a false origin which is determined relative to the intersection of the equator and the central meridian for each zone. For locations in the Northern Hemisphere, the false origin is 500,000 meters west of the central meridian on the equator. Coordinate measurements of location in the Northern Hemisphere using the UTM system are made relative to this point in meters in eastings(longitudinal distance) and northings (latitudinal distance). The point defined by the intersection of 50° North and 9° West would have a UTMcoordinate of Zone 29, 500000 meters east (E), 5538630 meters north (N) (see Figures 2b-4 and 2b-5). In the Southern Hemisphere, the origin is 10,000,000 meters south and 500,000 meters west of the equator and central meridian, respectively. The location found at 50° South and 9° West would have a UTM coordinate of Zone 29, 500000 meters E, 4461369 meters N (remember that northing in the Southern Hemisphere is measured from 10,000,000 meters south of the equator - see Figures 2b-4 and 2b-5).

Figure 2b-4: The following illustration describes the characteristics of the UTM zone "29" found between 12 to 6° West longitude. Note that the zone has been split into two halves. The half on the left represents the area found in the Northern Hemisphere. The Southern Hemisphere is located on the right. The blue line represents the central meridian for this zone. Locations measurements for this zone are calculated relative to a false origin. In the Northern Hemisphere, this origin is located 500,000 meters west of the equator. The Southern Hemisphere UTM measurements are determined relative to a origin located at 10,000,000 meters south and 500,000 meters west of the equator and central meridian, respectively.

The UTM system has been modified to make measurements less confusing. In this modification, the six degree wide zones are divided into smaller pieces or quadrilaterals that are eight degrees of latitude tall. Each of these rows is labeled, starting at 80° South, with the letters C to X consecutively with I and O being omitted (Figure 2b-5). The last row X differs from the other rows and extends from 72 to 84° North latitude (twelve degrees tall). Each of the quadrilaterals or grid zones are identified by their number/letter designation. In total, 1200 quadrilaterals are defined in the UTM system.

The quadrilateral system allows us to further define location using the UTM system. For the location 50° North and 9° West, the UTMcoordinate can now be expressed as Grid Zone 29U, 500000 meters E, 5538630 meters N.

Figure 2b-5: The UTM system also uses a grid system to break the Earth up into 1200 quadrilaterals. To keep the illustration manageable, most of these zones have been excluded. Designation of each quadrilaterals is accomplished with a number-letter system. Along the horizontal bottom, the six degree longitude wide zones are numbered, starting at 180° West longitude, from 1 to 60. The twenty vertical rows are assigned letters C to X with I and O excluded. The letter, C, begins at 80° South latitude. Note that the rows are 8 degrees of latitude wide, except for the last row X which is 12 degrees wide. According to the reference system, the bright green quadrilateral has the grid reference 29V (note that in this system west-east coordinate is given first, followed by the south-north coordinate). This grid zone is found between 56 and 64° North latitude and 6 and 12° West longitude.

Each UTM quadrilateral is further subdivided into a number of 100,000 by 100,000 meter zones. These subdivisions are coded by a system of letter combinations where the same two-letter combination is not repeated within 18 degrees of latitude and longitude. Within each of the 100,000 meter squares one can specify location to one-meter accuracy using a 5 digit eastings and northings reference system.

The UTM grid system is displayed on all United States Geological Survey (USGS) and National Topographic Series (NTS) of Canada maps. On USGS 7.5-minute quadrangle maps (1:24,000 scale), 15-minute quadrangle maps (1:50,000, 1:62,500, and standard-edition 1:63,360 scales), and Canadian 1:50,000 maps the UTM grid lines are drawn at intervals of 1,000 meters, and are shown either with blue ticks at the edge of the map or by full blue grid lines. On USGS maps at 1:100,000 and 1:250,000 scale and Canadian 1:250,000 scale maps a full UTM grid is shown at intervals of 10,000 meters. Figure 2b-6 describes how the UTM grid system can be used to determine location on a 1:50,000 National Topographic Series of Canada map.

Figure 2b-6: The top left hand corner the "Tofino" 1:50,000 National Topographic Series of Canada map is shown above. The blue lines and associated numbers on the map margin are used to determine location by way of the UTM grid system. Abbreviated UTM 1,000-meter values or principle digits are shown by numbers on the map margin that vary from 0 to 100 (100 is actually given the value 00). In each of the corners of the map, two of the principle digits are expressed in their full UTM coordinate form. On the image we can see 283000 m E. and 5458000 m N. The red dot is found in the center of the grid defined by principle numbers 85 to 86 easting and 57 to 58 northing. A more complete UTM grid reference for this location would be 285500 m E. and 5457500 m N. Information found on the map margin also tells us (not shown) that the area displayed is in Grid Zone 10U and the 100,000 m squares BK and CK are located on this map.

Distance on Maps

In section 2a, w e have learned that depicting the Earth's three-dimensional surface on a two-dimensional map creates a number of distortions that involve distance, area, and direction. It is possible to create maps that are somewhat equidistance. However, even these types of maps have some form of distance distortion. Equidistance maps can only control distortion along either lines of latitude or lines of longitude. Distance is often correct on equidistance maps only in the direction of latitude.

On a map that has a large scale, 1:125,000 or larger, distance distortion is usually insignificant. An example of a large-scale map is a standard topographic map. On these maps measuring straight line distance is simple. Distance is first measured on the map using a ruler. This measurement is then converted into a real world distance using the map's scale. For example, if we measured a distance of 10 centimeters on a map that had a scale of 1:10,000, we would multiply 10 (distance) by 10,000 (scale). Thus, the actual distance in the real world would be 100,000 centimeters.

Measuring distance along map features that are not straight is a little more difficult. One technique that can be employed for this task is to use a number of straight-line segments. The accuracy of this method is dependent on the number of straight-line segments used (Figure 2b-7). Another method for measuring curvilinear map distances is to use a mechanical device called an opisometer. This device uses a small rotating wheel that records the distance traveled. The recorded distance is measured by this device either in centimeters or inches.

Figure 2b-7: Measurement of distance on a map feature using straight-line segments.

Direction on Maps

Like distance, direction is difficult to measure on maps because of the distortion produced by projection systems. However, this distortion is quite small on maps with scales larger than 1:125,000. Direction is usually measured relative to the location of North or South Pole. Directions determined from these locations are said to be relative to True North or True South. The magnetic poles can also be used to measure direction. However, these points on the Earth are located in spatially different spots from the geographic North and South Pole. The North Magnetic Poleis located at 78.3° North, 104.0° West near Ellef Ringnes Island, Canada. In the Southern Hemisphere, the South Magnetic Pole is located in Commonwealth Day, Antarctica and has a geographical location of 65° South, 139° East. The magnetic poles are also not fixed overtime and shift their spatial position overtime.

Topographic maps normally have a declination diagram drawn on them (Figure 2b-8). On Northern Hemisphere maps, declination diagrams describe the angular difference between Magnetic North and True North. On the map, the angle of True North is parallel to the depicted lines of longitude. Declination diagrams also show the direction of Grid North. Grid North is an angle that is parallel to the easting lines found on theUniversal Transverse Mercator (UTM) grid system (Figure 2b-8).

Figure 2b-8: This declination diagram describes the angular difference between Grid, True, and Magnetic North. This illustration also shows how angles are measured relative grid, true, and magnetic azimuth.

In the field, the direction of features is often determined by a magnetic compass which measures angles relative to Magnetic North. Using the declination diagram found on a map, individuals can convert their field measures of magnetic direction into directions that are relative to either Grid or True North. Compass directions can be described by using either the azimuth system or the bearing system. The azimuth system calculates direction in degrees of a full circle. A full circle has 360 degrees (Figure 2b-9). In the azimuth system, north has a direction of either the 0 or 360°. East and west have an azimuth of 90° and 270°, respectively. Due south has an azimuth of 180°.

Figure 2b-9: Azimuth system for measuring direction is based on the 360 degrees found in a full circle. The illustration shows the angles associated with the major cardinal points of the compass. Note that angles are determined clockwise from north.

The bearing system divides direction into four quadrants of 90 degrees. In this system, north and south are the dominant directions. Measurements are determined in degrees from one of these directions. The measurement of two angles based on this system are described inFigure 2b-10.

Figure 2b-10: The bearing system uses four quadrants of 90 degrees to measure direction. The illustration shows two direction measurements. These measurements are made relative to either north or south. North and south are given the measurement 0 degrees. East and west have a value of 90 degrees. The first measurement (green) is found in the north - east quadrant. As a result, its measurement is north 75 degrees to the east or N75°E. The first measurement (orange) is found in the south - west quadrant. Its measurement is south 15 degrees to the west or S15°W.

Global Positioning Systems

Determination of location in field conditions was once a difficult task. In most cases, it required the use of a topographic map and landscape features to estimate location. However, technology has now made this task very simple. Global Positioning Systems (GPS) can calculate one's location to an accuracy of about 30-meters (Figure 2b-11). These systems consist of two parts: a GPS receiver and a network of many satellites. Radio transmissions from the satellites are broadcasted continually. The GPS receiver picks up these broadcasts and through triangulation calculates the altitude and spatial position of the receiving unit. A minimum of three satellite is required for triangulation.

Figure 2b-11: Handheld Global Positioning Systems (GPS). GPS receivers can determine latitude, longitude, and elevation anywhere on or above the Earth's surface from signals transmitted by a number of satellites. These units can also be used to determine direction, distance traveled, and determine routes of travel in field situations.

source: http://www.physicalgeography.net/fundamentals/2b.html

Quadrotor with IMU

2013-04-18T01:05:00.000+10:00

Quadrotor

A quadrotor, also called a quadrotor helicopter or quadrocopter, is an aircraft that is lifted and propelled by four rotors. Following shows the schematic of reaction torques on each motor of a quadrotor aircraft, due to spinning rotors.

In quadrotor system, each rotor produces both a thrust and torque about its center of rotation, as well as a drag force opposite to the vehicle's direction of flight. If all rotors are spinning at the same angular velocity, with rotors one and three rotating clockwise and rotors two and four counterclockwise, the net aerodynamic torque, and hence the angular acceleration about the yaw axis is exactly zero, which implies that the yaw stabilizing rotor of conventional helicopters is not needed. Yaw is induced by mismatching the balance in aerodynamic torques. Angular accelerations about the pitch and roll axes can be caused separately without impacting the yaw axis. Each pair of blades rotating in the same direction controls one axis, either roll or pitch, and increasing thrust for one rotor while decreasing thrust for the other will maintain the torque balance needed for yaw stability and induce a net torque about the roll or pitch axes. This way, fixed rotor blades can be made to maneuver the quad rotor vehicle in all dimensions. Translational acceleration is achieved by maintaining a non-zero pitch or roll angle. Based on these flight control method, following control block diagram can be used.

MLE (negative log likelihood)

2013-04-16T19:41:00.000+10:00

Why Minimize Negative Log Likelihood?

One of the wonders of machine learning is the diversity of divergent traditions from which it originates, from classical statistics (both frequentist and Bayesian) to information and control theories, plus a significant dose of pragmatism from computer science. For those interested in the historical relationship between statistics and machine learning, see Breiman’s Two Cultures.

This diversity is reflected in the surprising complexity in answering simple-sounding questions, which often speaks to the heart of trading using computational machine learning models—ranging from estimating HMM models via MLE (e.g. vol / correlation regime models) to non-convex optimization via non-standard likelihood or loss functions (e.g. portfolio optimization viaomega):

Why is minimizing the negative log likelihood equivalent to maximum likelihood estimation (MLE)?

Or, equivalently, in Bayesian-speak:

Why is minimizing the negative log likelihood equivalent to maximum a posteriori probability (MAP), given a uniform prior?

Answering this question provides insight into the foundations of machine learning, as well as connection with several branches of mathematics.

Classic statistics opens the answer, beginning with the definition of a likelihood function:

Applying the natural log function in this context is handy, for several reasons. First, numerical analysis reminds us that logs reduce potential for underflow, due to very small likelihoods. Second, calculus reminds us logs permit the addition trick: converting a product of factors into a summation of factors (as seen before in Why Log Returns?). Finally, calculus again reminds us that the natural log function is a monotone transformation.

Thus, the extrema of are equivalent to the extrema of :

From which the maximum likelihood estimator is defined as:

As an aside, Bayesians will remind us we can generalized into a MAP estimator, given uniform prior :

From which optimization and real analysis reminds us of the following equivalence, for all :

Thus, the following are equivalent:

Pan Tilt mount, Their Terminology

2013-04-14T20:56:00.002+10:00

Pan == > Yaw angle

Tilt ==> Pitch

Bank ==> Stable Roll

Complimentary AHRS approach (DCM algorithm)

2013-04-14T17:59:00.000+10:00

(Source: http://www.starlino.com/dcm_tutorial.html)

Introduction

This article is a continuation of my IMU Guide, covering additional orientation kinematics topics. I will go through some theory first and then I will present a practical example with code build around an Arduino and a 6DOF IMU sensor (acc_gyro_6dof). The scope of this experiment is to create an algorithm for fusing gyroscope and accelerometer data in order to create an estimation of the device orientation in space. Such an algorithm was already presented in part 3 of my “IMU Guide” and a practical Arduino experiment with code was presented in the “Using a 5DOF IMU” article and was nicknamed “Simplified Kalman Filter”, providing a simple alternative to the well known Kalman Filter algorithm. In this article we’ll use another approach utilizing the DCM (Direction Cosine Matrix). For the reader that is unfamiliar with MEMS sensors it is recommended to read Part 1 and 2 of the IMU Guide article. Also for following the experiments presented in this text it is recommended to acquire an Arduino board and an acc_gyro_6dof sensor.

Prerequisites

No really advanced math is necessary. Find a good book on matrix operations, that’s all you might need above school math course. If you would like to refresh your knowledge below are some quick articles:
Cartesian Coordinate System – http://en.wikipedia.org/wiki/Cartesian_coordinate_system
Rotation – http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
Vector scalar product – http://en.wikipedia.org/wiki/Dot_product
Vector cross product – http://en.wikipedia.org/wiki/Cross_product
Matrix Multiplication – http://en.wikipedia.org/wiki/Matrix_multiplication
Block Matrix – http://en.wikipedia.org/wiki/Block_matrix
Transpose Matrix – http://en.wikipedia.org/wiki/Transpose
Triple Product – http://en.wikipedia.org/wiki/Triple_product

Notations

Vectors are marked in bold text - so for example “v” is a vector and “v” is a scalar (if you can’t distinguish the two there’s problem with the text formatting wherever you’re reading this).

Part 1. The DCM Matrix

Generally speaking orientation kinematics deals with calculating the relative orientation of a body relative to a global coordinate system. It is useful to attach a coordinate system to our body frame and call it Oxyz, and another one to our global frame and call it OXYZ. Both the global and the body frames have the same fixed origin O (see Fig. 1). Let’s also define i, j, k to be unity vectors co-directional with the body frame’s x, y, and z axes – in other words they are versors of Oxyz and let I, J, K be the versors of global frame OXYZ.

Figure 1

Thus, by definition, expressed in terms of global coordinates vectors I, J, K can be written as:

I^G = {1,0,0}^T, J^G={0,1,0}^T , K^G = {0,0,1}^T

Note: we use {…}^Tnotation to denote a column vector, in other words a column vector is a translated row vector. The orientation of vectors (row/column) will become relevant once we start multiplying them by a matrix later on in this text.

And similarly, in terms of body coordinates vectors i, j, k can be written as:

i^B = {1,0,0}^T, j^B={0,1,0}^T , k^B = {0,0,1}^T

Now let’s see if we can write vectors i, j, k in terms of global coordinates. Let’s take vector i as an example and write its global coordinates:

i^G = {i_x^G , i_y^G, i_z^G}^T

Again, by example let’s analyze the X coordinate i_x^G, it’s calculated as the length of projection of the i vector onto the global X axis.

i_x^G= |i| cos(X,i) = cos(I,i)

Where |i| is the norm (length) of the i unity vector and cos(I,i) is the cosine of the angle formed by the vectors Iand i. Using the fact that |I| = 1 and |i| = 1 (they are unit vectors by definition). We can write:

i_x^G= cos(I,i) = |I||i| cos(I,i) = I.i

Where I.i. is the scalar (dot) product of vectors I and i. For the purpose of calculating scalar product I.i it doesn’t matter in which coordinate system these vectors are measured as long as they are both expressed in the same system, since a rotation does not modify the angle between vectors so: I.i = I^B.i^B = I^G.i^G= cos(I^B.i^B) = cos(I^G.i^G) , so for simplicity we’ll skip the superscript in scalar products I.i , J.j , K.k and in cosines cos(I,i), cos(J,j), cos(K,k).

Similarly we can show that:

i_y^G = J.i, i_z^G=K.i , so now we can write vector i in terms of global coordinate system as:

i^G= { I.i, J.i, K.i}^T

Furthermore, similarly it can be shown that j^G= { I.j, J.j, K.j}^T , k^G= { I.k, J.k, K.k}^T.

We now have a complete set of global coordinates for our body’s versors i, j, k and we can organize these values in a convenient matrix form:

(Eq. 1.1)

This matrix is called Direction Cosine Matrix for now obvious reasons – it consists of cosines of angles of all possible combinations of body and global versors.

The task of expressing the global frame versors I^G, J^G, K^G in body frame coordinates is symmetrical in nature and can be achieved by simply swapping the notations I, J, K with i, j, k, the results being:

I^B= { I.i, I.j, I.k}^T , J^B= { J.i, J.j, J.k}^T , K^B= { K.i, K.j, K.k}^T

and organized in a matrix form:

(Eq. 1.2)

It is now easy to notice that DCM^B = (DCM^G)^Tor DCM^G = (DCM^B)^T, in other words the two matrices are translates of each other, we’ll use this important property later on.

Also notice that DCM^B. DCM^G = (DCM^G)^T .DCM^G = DCM^B. (DCM^B)^T = I₃ , where I₃ is the 3×3 identity matrix. In other words the DCM matrices are orthogonal.

This can be proven by simply expanding the matrix multiplication in block matrix form:

(Eq. 1.3)

To prove this we use such properties as for example: i^GT. i^G = | i^G|| i^G|cos(0) = 1 and i^GT. j^G = 0 because (i andj are orthogonal) and so forth.

The DCM matrix (also often called the rotation matrix) has a great importance in orientation kinematics since it defines the rotation of one frame relative to another. It can also be used to determine the global coordinates of an arbitrary vector if we know its coordinates in the body frame (and vice versa).

Let’s consider such a vector with body coordinates:

r^B= { r_x^B, r_y^B, r_z^B}^T and let’s try to determine its coordinates in the global frame, by using a known rotation matrix DCM^G.

We start by doing following notation:

r^G = { r_x^G , r_y^G , r_z^G } ^T.

Now let’s tackle the first coordinate r_x^G:

r_x^G = | r^G| cos(I^G,r^G) , because r_x^G is the projection of r^G onto X axis that is co-directional with I^G.

Next let’s note that by definition a rotation is such a transformation that does not change the scale of a vector and does not change the angle between two vectors that are subject to the same rotation, so if we express some vectors in a different rotated coordinate system the norm and angle between vectors will not change:

| r^G| = | r^B| , | I^G| = | I^B| = 1 and cos(I^G,r^G) = cos(I^B,r^B), so we can use this property to write

r_x^G = | r^G| cos(I^G,r^G) = | I^B || r^B| cos(I^B,r^B) = I^B. r^B = I^B. { r_x^B, r_y^B, r_z^B}^T , by using one the two definition of the scalar product.

Now recall that I^B= { I.i, I.j, I.k}^T and by using the other definition of scalar product:

r_x^G = I^B. r^B = { I.i, I.j, I.k}^T . { r_x^B, r_y^B, r_z^B}^T = r_x^B I.i + r_y^B I.j + r_z^B I.k

In same fashion it can be shown that:

r_y^G = r_x^B J.i + r_y^B J.j + r_z^B J.k r_z^G = r_x^B K.i + r_y^B K.j + r_z^B K.k

Finally let’s write this in a more compact matrix form:

(Eq. 1.4)

Thus the DCM matrix can be used to covert an arbitrary vector r^B expressed in one coordinate system B, to a rotated coordinate system G.

We can use similar logic to prove the reverse process:

(Eq. 1.5)

Or we can arrive at the same conclusion by multiplying both parts in (Eq. 1.4) by DCM^B which equals to DCM^GT, and using the property that DCM^GT.DCM^G = I₃ , see (Eq. 1.3):

DCM^B r^G = DCM^B DCM^G r^B = DCM^GT DCM^G r^B = I₃ r^B= r^B

Part 2. Angular Velocity

So far we have a way to characterize the orientation of one frame relative to another rotated frame, it is the DCM matrix and it allows us to easily convert the global and body coordinates back and forth using (Eq. 1.4) and (Eq. 1.5). In this section we’ll analyze the rotation as a function of time that will help us establish the rules of updating the DCM matrix based on a characteristic called angular velocity. Let’s consider an arbitrary rotating vector rand define it’s coordinates at time t to be r(t). Now let’s consider a small time interval dt and make the following notations: r = r (t) , r’= r (t+dt) and dr = r’ – r:

Figure 2

Let’s say that during a very small time interval dt → 0 the vector r has rotated about an axis co-directional with a unity vector uby an angle dθ and ended up in the position r’. Since u is our axis of rotation it is perpendicular to the plane in which the rotation took place (the plane formed by r and r’) so u is orthogonal to both r and r’. There are two unity vectors that are orthogonal to the plane formed by r and r’, they are shown on the picture asu and u’ since we’re still defining things we’ll choose the one that is co-directional with the cross product r x r’, following the rule of right-handed coordinate system. Thus because u is a unity vector |u| = 1 and is co-directional with r x r’ we can deduct it as follows:

u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|² sin(dθ)) (Eq. 2.1)

Since a rotation does not alter the length of a vector we used the property that| r’| = |r|.

The linear velocity of the vector r can be defined as the vector:

v = dr / dt = ( r’ – r) / dt (Eq. 2.2)

Please note that since our dt approaches 0 so does dθ → 0, hence the angle between vectors r and dr (let’s call it α) can be found from the isosceles triangle contoured by r , r’ and dr:

α = (π – dθ) / 2 and because dθ → 0 , then α → π/2

What this tells us is that r is perpendicular to dr when dt → 0 and hence r is perpendicular to v since v and drare co-directional from (Eq. 2.2):

v ⊥ r (Eq. 2.21)

We are now ready to define the angular velocity vector. Ideally such a vector should define the rate of change of the angle θ and the axis of the rotation, so we define it as follows:

w = (dθ/dt ) u (Eq. 2.3)

Indeed the norm of the w is |w| = dθ/dt and the direction of w coincides with the axis of rotation u. Let’s expand(Eq. 2.3) and try to establish a relationship with the linear velocity v:

Using (Eq. 2.3) and (Eq. 2.1):

w = (dθ/dt ) u = (dθ/dt ) (r x r’) / (|r|² sin(dθ))

Now note that when dt → 0, so does dθ → 0 and hence for small dθ, sin(dθ) ≈ dθ , we end up with:

w = (r x r’) / (|r|² dt) (Eq. 2.4)

Now because r’ = r + dr , dr/dt = v , r x r = 0 and using the distributive property of cross product over addition:

w = (r x (r + dr)) / (|r|² dt) = (r x r + r x dr)) / (|r|² dt) = r x (dr/dt) / |r|²

And finally:

w = r x v / |r|² (Eq. 2.5)

This equation establishes a way to calculate angular velocity from a known linear velocity v.

We can easily prove the reverse equation that lets us deduct linear velocity from angular velocity:

v = w x r (Eq. 2.6)

This can be proven simply by expanding w from (Eq. 2.5) and using vector triple product rule (a x b) x c = (a.c)b– (b.c)a. Also we’ll use the fact that v and r are perpendicular (Eq. 2.21) and thus v.r = 0

w x r = (r x v / |r|²) x r = (r x v) x r / |r|² = ((r.r) v + (v.r)r) / |r|² = ( |r|² v + 0) |r|²= v

So we just proved that (Eq. 2.6) is true. Just to check (Eq. 2.6) intuitively – from Figure 2 indeed v has the direction of w x r using the right hand rule and indeed v ⊥ r and v ⊥ w because it is in the same plane with rand r’.

Part 3. Gyroscopes and angular velocity vector

A 3-axis MEMS gyroscope is a device that senses rotation about 3 axes attached to the device itself (body frame). If we adopt the device’s coordinate system (body’s frame), and analyze some vectors attached to the earth (global frame), for example vector K pointing to the zenith or vector I pointing North – then it would appear to an observer inside the device that these vector rotate about the device center. Let w_x, w_y, w_zbe the outputs of a gyroscope expressed in rad/s – the measured rotation about axes x, y , z respectively. Converting from the raw output of the gyroscope to physical values is discussed for example here:http://www.starlino.com/imu_guide.html . If we query the gyroscope at regular, small time intervals dt, then what gyroscope output tells us is that during this time interval the earth rotated about gyroscope’s x axis by an angle of dθ_x = w_xdt, about y axis by an angle of dθ_y = w_ydt and about z axis by an angle of dθ_z = w_zdt. These rotations can be characterized by the angular velocity vectors: w_x= w_x i = {w_x, 0, 0}^T, w_y= w_y j = { 0, w_y, 0}^T, w_z= w_z k = { 0, 0, w_z}^T , where i,j,k are versors of the local coordinate frame (they are co-directional with body’s axes x,y,z respectively). Each of these three rotations will cause a linear displacement which can be expressed by using (Eq. 2.6):

dr₁ = dt v₁ = dt (w_x x r) ; dr₂ = dt v₂ = dt (w_y x r) ; dr₃ = dt v₃ = dt (w_z x r) .

The combined effect of these three displacements will be:

dr = dr₁+ dr₂+ dr₃= dt (w_x x r + w_y x r + w_z x r) = dt (w_x + w_y + w_z) x r (cross product is distributive over addition)

Thus the equivalent linear velocity resulting from these 3 transformations can be expressed as:

v = dr/dt = (w_x + w_y + w_z) x r = w x r , where we introduce w = w_x + w_y + w_z = {w_x, w_y, w_z}

Which looks exactly like (Eq. 2.6) and suggests that the combination of three small rotations about axes x,y,z characterized by angular rotation vectors w_x , w_y , w_z is equivalent to one small rotation characterized by angular rotation vector w = w_x + w_y + w_z = {w_x, w_y, w_z}. Please note that we’re stressing out that these aresmall rotations, since in general when you combine large rotations the order in which rotations are performed become important and you cannot simply sum them up. Our main assumption that let us go from a linear displacement to a rotation by using (Eq. 2.6) was that dt is really small, and thus the rotations dθand linear displacement dr are small as well. In practice this means that the larger the dt interval between gyro queries the larger will be our accumulated error, we’ll deal with this error later on. Now, since w_x, w_y, w_zare the output of the gyroscope, then we arrive at the conclusion that in fact a 3 axis gyroscope measures the instantaneous angular velocity of the world rotating about the device’s center.

Part 4. DCM complimentary filter algorithm using 6DOF or 9DOF IMU sensors

In the context of this text a 6DOF device is an IMU device consisting of a 3 axis gyroscope and a 3 axis accelerometer. A 9DOF device is an IMU device of a 3 axis gyroscope, a 3 axis accelerometer and a 3 axis magnetometer. Let’s attach a global right-handed coordinate system to the Earth’s frame such that the I versor points North, K versor points to the Zenith and thus, with these two versors fixed, the J versor will be constrained to point West.

Figure 3

Also let’s consider the body coordinate system to be attached to our IMU device (acc_gyro used as an example),

Figure 4

We already established the fact that gyroscopes can measure the angular velocity vector. Let’s see how accelerometer and magnetometer measurements will fall into our model.

Accelerometers are devices that can sense gravitation. Gravitation vector is pointing towards the center of the earth and is opposite to the vector pointing to Zenith K^B. If the 3 axis accelerometer output is A = {A_x, Ay , A_z } and we assume that there are no external accelerations or we have corrected them then we can estimate that K^B= -A. (See this IMU Guide for more clarifications http://www.starlino.com/imu_guide.html).

Magnetometers are devices that are really similar to accelerometers, except that instead of gravitation they can sense the Earth’s magnetic North. Just like accelerometers they are not perfect and often need corrections and initial calibration. If the corrected 3-axis magnetometer output is M = {M_x, My , M_z }, then according to our model I^B is pointing North , thus I^B = M.

Knowing I^B and K^B allows us calculate J^B = K^B x I^B.

Thus an accelerometer and a magnetometer alone can give us the DCM matrix , expressed either as DCM^B or DCM^G

DCM^G = DCM^BT = [I^B, J^B, K^B]^T

The DCM matrix can be used to convert any vector from body’s(devices) coordinate system to the global coordinate system. Thus for example if we know that the nose of the plane has some fixed coordinates expressed in body’s coordinate system as r^B = {1,0,0}, the we can find where the device is heading in other words the coordinates of the nose in global coordinate systems using (Eq. 1.4):

r^G = DCM^G r^B

So far you’re asking yourself if an accelerometer and a magnetometer gives us the DCM matrix at any point in time, why do we need the gyroscope ? The gyroscope is actually a more precise device than the accelerometer and magnetomer are , it is used to “fine-tune” the DCM matrix returned by the accelerometer and magnetometer.

Gyroscopes have no sense of absolute orientation of the device , i.e. they don’t know where north is and where zenith is (things that we can find out using the accelerometer and magnetometer), instead if we know the orientation of the device at time t, expressed as a DCM matrix DCM(t) , we can find a more precise orientation DCM(t+dt) using the gyroscope , then the one estimated directly from the accelerometer and magnetometer direct readings which are subject to a lot of noise in form of external (non-gravitational) inertial forces (i.e. acceleration) or magnetically forces that are not caused by the earth’s magnetic field.

These facts call for an algorithm that would combine the readings from all three devices (accelerometer, magnetometer and gyroscope) in order to create our best guess or estimate regarding the device orientation in space (or space’s orientation in device’s coordinate systems), the two orientations are related since they are simply expressed using two DCM matrices that are transpose of one another (DCM^G = DCM^BT).

We’ll now go ahead and introduce such an algorithm.

We’ll work with the DCM matrix that consists of the versors of the global (earth’s) coordinate system aligned on each row:

If we read the rows of DCM^G we get the vectors I^B, J^B, K^B. We’ll work mostly with vectors K^B (that can be directly estimated by accelerometer) and vector I^B(that can be directly estimated by the magnetometer). The vector J^Bis simply calculated as J^B = K^B x I^B , since it’s orthogonal to the other two vectors (remember versors are unity vectors with same direction as coordinate axes).

Let’s say we know the zenith vector expressed in body frame coordinates at time t₀ and we note it as K^B_0.Also let’s say we measured our gyro output and we have determined that our angular velocity is w = {w_x, w_y, w_z}. Using our gyro we want to know the position of our zenith vector after a small period of time dt has passed we’ll note it as K^B_1G. And we find it using (Eq. 2.6):

K^B_1G ≈ K^B₀ + dt v = K^B₀ + dt (w_g x K^B₀) = K^B₀ + ( dθ_g x K^B₀)

Where we noted dθ_g = dt w_g. Because w_g is angular velocity as measured by the gyroscope. We’ll call dθ_gangular displacement. In other words it tells us by what small angle (given for all 3 axis in form of a vector) has the orientation of a vector K^B changed during this small period of time dt.

Obviously, another way to estimate K^B is by making another reading from accelerometer so we can get a reading that we note as K^B_1A.

In practice the values K^B_1G will be different from from K^B_1A.One was estimated using our gyroscope and the other was estimated using our accelerometer.

Now it turns out we can go the reverse way and estimate the angular velocity w_a or angular displacement dθ_a=dt w_a , from the new accelerometer reading K^B_1A, we’ll use (Eq. 2.5):

w_a = K^B₀ x v_a / | K^B₀|²

Now v_a= (K^B_1A- K^B₀) / dt , and is basically the linear velocity of the vector K^B₀. And | K^B₀|² = 1 , since K^B₀ is a unity vector. So we can calculate:

dθ_a= dt w_a= K^B₀ x (K^B_1A- K^B₀)

The idea of calculating a new estimate K^B₁ that combines both K^B_1A and K^B_1G is to first estimate dθ as a weighted average of dθ_a and dθ_g :
dθ = (s_a dθ_a + s_g dθ_g) / (s_a + s_g), we’ll discuss about the weights later on , but shortly they are determined and tuned experimentally in order to achieve a desired response rate and noise rejection.

And then K^B₁is calculated similar to how we calculated K^B_1G:

K^B₁ ≈ K^B₀ + ( dθ x K^B₀)

Why we went all the way to calculate dθ and did not apply the weighted average formula directly to K^B_1A andK^B_1G? Because dθ can be used to calculate the other elements of our DCM matrix in the same way:

I^B₁ ≈ I^B₀ + ( dθ x I^B₀)

J^B₁ ≈ J^B₀ + ( dθ x J^B₀)

The idea is that all three versors I^B, J^B, K^B are attached to each other and will follow the same angular displacement dθ during our small interval dt. So in a nutshell this is the algorithm that allows us to calculate the DCM₁ matrix at time t₁from our previous estimated DCM₀matrix at time t₀. It is applied recursively at regular small time intervals dt and gives us an updated DCM matrix at any point in time. The matrix will not drift too much because it is fixed to the absolute position dictated by the accelerometer and will not be too noisy from external accelerations because we also use the gyroscope data to update it.

So far we didn’t mention a word about our magnetometer. One reasons being that it is not available on all IMU units (6DOF) and we can go away without using it, but our resulting orientation will then have a drifting heading (i.e. it will not show if we’re heading north, south, west or east), or we can introduce a virtual magnetometer that is always pointing North, to introduce stability in our model. This situation is demonstrated in the accompanying source code that used a 6DOF IMU.

Now we’ll show how to integrate magnetometer readings into our algorithm. As it turns out it is really simple since magnetometer is really similar to accelerometer (they even use similar calibration algorithms), the only difference being that instead of estimating the Zenith vector K^Bvector it estimates the vector pointing North I^B. Following the same logic as we did for our accelerometer we can determine the angular displacement according to the updated magnetometer reading as being:

dθ_m= dt w_m= I^B₀ x (I^B_1M- I^B₀)

Now let’s incorporate it into our weighted average:

dθ = (s_a dθ_a + s_g dθ_g+ s_m dθ_m) / (s_a + s_g +s_m)

From here we go the same path to calculate the updated DCM₁

I^B₁ ≈ I^B₀ + ( dθ x I^B₀) , K^B₁ ≈ K^B₀ + ( dθ x K^B₀) and J^B₁≈ J^B₀ + ( dθ x J^B₀),

In practice we’ll calculate J^B₁= K^B₁x I^B_1, after correcting K^B₁and I^B₁to be perpendicular unity vectors again , note that all our logic is approximated and dependent on dt being small, the larger the dt the larger the error we’ll accumulate.

So if vectors I^B₀, J^B₀, K^B₀form a valid DCM matrix , in other words they are orthogonal to each other and are unity vectors, then we can’t say the same about I^B₁, J^B₁, K^B₁, the formulas used for calculating them does not guarantee the orthogonality or length of the vector to be preserved , however we will not get a big error if dt is small, all we need to do is to correct them after each iteration.

First let’s see how we can ensure that two vectors are orthogonal again. Let’s consider two unity vectors a and bthat are “almost orthogonal” in other words the angle between these two vectors is close to 90°, but not exactly 90°. We’re looking to find a vector b’ that is orthogonal to a and that is in the same plane formed by the vectors aand b. Such a vector is easy to find as shown in Figure 5. First we find vector c = a x b that by the rules of cross product is orthogonal to both a and b and thus is perpendicular to the plane formed by a and b. Next the vectorb’ = c x a is calculated as the cross product of c and a. From the definition of cross product b’ is orthogonal to aand because it is also orthogonal to c – it end up in the plane orthogonal to c , which is the plane formed by a andb. Thus b’ is the corrected vector we’re seeking that is orthogonal to a and belongs to the plane formed by a andb.

Figure 5

We can extend the equation using the triple product rule and the fact that a.a = |a| = 1:

b’ = c x a = (a x b) x a = -a (a.b) + b(a.a) = b – a (a.b) = b + d , where d = – a (a.b) (Scenario 1, a is fixed b is corrected)

You can reflect a little bit on the results … So we obtain corrected vector b’ from vector b by adding a “correction” vector d = – a (a.b). Notice that d is parallel to a. Its direction is dependent upon the angle between a and b, for example in Figure 5 a.b = cos (a,b) > 0 , because angle between a and b is less than 90°thusd has opposite direction from a and a magnitutde of cos(a,b) = sin(b,b’).

In the scenario above we considered that vector a is fixed and we found a corrected vector b’ that is orthogonal to a. We can consider the symmetric problem – we fix b and find the corrected vector a’:

a’ = a – b (b.a) = a – b (a.b) = a + e, where e = - b (a.b) (Scenario 2, b is fixed a is corrected)

Finally in the third scenario we want both vectors to move towards their corrected state, we consider them both “equally wrong”, so intuitively we apply half correction to both vectors from scenario 1 and 2:

a’ = a – b (a.b) / 2 (Scenario 3, both a and b are corrected)
b’ = b – a (a.b) / 2

Figure 6

This is an relatively easy formula to calculate on a microprocessor since we can pre-compute Err = (a.b)/2 and then use it to correct both vectors:

a’ = a – Err * b
b’ = b – Err * a

Please note that we’re not proving that a’ and b’ are orthogonal in Scenario 3, but we presented the intuitive reasoning why the angle between a’ and b’ will get closer to 90°if we apply the above corrective transformations.

Now going back to our updated DCM matrix that consists of three vectors I^B₁, J^B₁, we apply the following corrective actions before reintroducing the DCM matrix into the next loop:

Err = ( I^B₁ . J^B₁) / 2

I^B₁^’= I^B₁ – Err * J^B₁J^B₁^’= J^B₁ – Err * I^B₁I^B₁^’’= Normalize[I^B₁^’]
J^B₁^’’= Normalize[J^B₁^’]
K^B₁^’’= I^B₁^’’ x J^B₁^’’

Where Normalize[a] = a / |a| , is the formula calculating the unit vector co-directional with a.

So finally our corrected DCM₁ matrix can be recomposed from vectors I^B₁^’’, J^B₁^’’, K^B₁^’’that have been ortho-normalized (each vector constitutes a row of the updated and corrected DCM matrix).

We repeat the loop to find DCM_{2 ,} DCM_{3 ,} or in general DCM_n, at any time interval n.

References

1. Theory of Applied Robotics: Kinematics, Dynamics, and Control (Reza N. Jazar)

2. Linear Algebra and Its Applications (David C. Lay)

3. Fundamentals of Matrix Computations (David S. Watkins)

4. Direction Cosine Matrix IMU: Theory (W Premerlani)

IMU Guide

2013-04-14T17:58:00.001+10:00

(source is: http://www.starlino.com/imu_guide.html)

Part 1. Accelerometer

To understand this unit we'll start with the accelerometer. When thinking about accelerometers it is often useful to image a box in shape of a cube with a ball inside it. You may imagine something else like a cookie or a donut , but I'll imagine a ball:

If we take this box in a place with no gravitation fields or for that matter with no other fields that might affect the ball's position – the ball will simply float in the middle of the box. You can imagine the box is in outer-space far-far away from any cosmic bodies, or if such a place is hard to find imagine at least a space craft orbiting around the planet where everything is in weightless state . From the picture above you can see that we assign to each axis a pair of walls (we removed the wall Y+ so we can look inside the box). Imagine that each wall is pressure sensitive. If we move suddenly the box to the left (we accelerate it with acceleration 1g = 9.8m/s^2), the ball will hit the wall X-. We then measure the pressure force that the ball applies to the wall and output a value of -1g on the X axis.

Please note that the accelerometer will actually detect a force that is directed in the opposite direction from the acceleration vector. This force is often called Inertial Force or Fictitious Force . One thing you should learn from this is that an accelerometer measures acceleration indirectly through a force that is applied to one of it's walls (according to our model, it might be a spring or something else in real life accelerometers). This force can be caused by the acceleration , but as we'll see in the next example it is not always caused by acceleration.

If we take our model and put it on Earth the ball will fall on the Z- wall and will apply a force of 1g on the bottom wall, as shown in the picture below:

In this case the box isn't moving but we still get a reading of -1g on the Z axis. The pressure that the ball has applied on the wall was caused by a gravitation force. In theory it could be a different type of force – for example, if you imagine that our ball is metallic, placing a magnet next to the box could move the ball so it hits another wall. This was said just to prove that in essence accelerometer measures force not acceleration. It just happens that acceleration causes an inertial force that is captured by the force detection mechanism of the accelerometer.

While this model is not exactly how a MEMS sensor is constructed it is often useful in solving accelerometer related problems. There are actually similar sensors that have metallic balls inside, they are called tilt switches, however they are more primitive and usually they can only tell if the device is inclined within some range or not, not the extent of inclination.

So far we have analyzed the accelerometer output on a single axis and this is all you'll get with a single axis accelerometers. The real value of triaxial accelerometers comes from the fact that they can detect inertial forces on all three axes. Let's go back to our box model, and let's rotate the box 45 degrees to the right. The ball will touch 2 walls now: Z- and X- as shown in the picture below:

The values of 0.71 are not arbitrary, they are actually an approximation for SQRT(1/2). This will become more clear as we introduce our next model for the accelerometer.

In the previous model we have fixed the gravitation force and rotated our imaginary box. In last 2 examples we have analyzed the output in 2 different box positions, while the force vector remained constant. While this was useful in understanding how the accelerometer interacts with outside forces, it is more practical to perform calculations if we fix the coordinate system to the axes of the accelerometer and imagine that the force vector rotates around us.

Please have a look at the model above, I preserved the colors of the axes so you can make a mental transition from the previous model to the new one. Just imagine that each axis in the new model is perpendicular to the respective faces of the box in the previous model. The vector R is the force vector that the accelerometer is measuring (it could be either the gravitation force or the inertial force from the examples above or a combination of both). Rx, Ry, Rz are projection of the R vector on the X,Y,Z axes. Please notice the following relation:

R^2 = Rx^2 + Ry^2 + Rz^2 (Eq. 1)

which is basically the equivalent of the Pythagorean theorem in 3D.

Remember that a little bit earlier I told you that the values of SQRT(1/2) ~ 0.71 are not random. If you plug them in the formula above, after recalling that our gravitation force was 1 g we can verify that:

1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2

simply by substituting R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2) in Eq.1

After a long preamble of theory we're getting closer to real life accelerometers. The values Rx, Ry, Rz are actually linearly related to the values that your real-life accelerometer will output and that you can use for performing various calculations.

Before we get there let's talk a little about the way accelerometers will deliver this information to us. Most accelerometers will fall in two categories: digital and analog. Digital accelerometers will give you information using a serial protocol like I2C , SPI or USART, while analog accelerometers will output a voltage level within a predefined range that you have to convert to a digital value using an ADC (analog to digital converter) module. I will not go into much detail about how ADC works, partly because it is such an extensive topic and partly because it is different from one platform to another. Some microcontroller will have a built-in ADC modules some of them will need external components in order to perform the ADC conversions. No matter what type of ADC module you use you'll end up with a value in a certain range. For example a 10-bit ADC module will output a value in the range of 0..1023, note that 1023 = 2^10 -1. A 12-bit ADC module will output a value in the range of 0..4095, note that 4095 = 2^12-1.

Let's move on by considering a simple example, suppose our 10bit ADC module gave us the following values for the three accelerometer channels (axes):

AdcRx = 586
AdcRy = 630
AdcRz = 561

Each ADC module will have a reference voltage, let's assume in our example it is 3.3V. To convert a 10bit adc value to voltage we use the following formula:

VoltsRx = AdcRx * Vref / 1023

A quick note here: that for 8bit ADC the last divider would be 255 = 2 ^ 8 -1 , and for 12bit ADC last divider would be 4095 = 2^12 -1.

Applying this formula to all 3 channels we get:

VoltsRx = 586 * 3.3V / 1023 =~ 1.89V (we round all results to 2 decimal points)
VoltsRy = 630 * 3.3V / 1023 =~ 2.03V
VoltsRz = 561 * 3.3V / 1023 =~ 1.81V

Each accelerometer has a zero-g voltage level, you can find it in specs, this is the voltage that corresponds to 0g. To get a signed voltage value we need to calculate the shift from this level. Let's say our 0g voltage level is VzeroG = 1.65V. We calculate the voltage shifts from zero-g voltage as follows::

DeltaVoltsRx = 1.89V – 1.65V = 0.24V
DeltaVoltsRy = 2.03V – 1.65V = 0.38V
DeltaVoltsRz = 1.81V – 1.65V = 0.16V

We now have our accelerometer readings in Volts , it's still not in g (9.8 m/s^2), to do the final conversion we apply the accelerometer sensitivity, usually expressed in mV/g. Lets say our Sensitivity = 478.5mV/g = 0.4785V/g. Sensitivity values can be found in accelerometer specifications. To get the final force values expressed in g we use the following formula:

Rx = DeltaVoltsRx / Sensitivity

Rx = 0.24V / 0.4785V/g =~ 0.5g
Ry = 0.38V / 0.4785V/g =~ 0.79g
Rz = 0.16V / 0.4785V/g =~ 0.33g

We could of course combine all steps in one formula, but I went through all the steps to make it clear how you go from ADC readings to a force vector component expressed in g.

Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity (Eq.2)
Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

We now have all 3 components that define our inertial force vector, if the device is not subject to other forces other than gravitation, we can assume this is the direction of our gravitation force vector. If you want to calculate inclination of device relative to the ground you can calculate the angle between this vector and Z axis. If you are also interested in per-axis direction of inclination you can split this result into 2 components: inclination on the X and Y axis that can be calculated as the angle between gravitation vector and X / Y axes. Calculating these angles is more simple than you might think, now that we have calculated the values for Rx,Ry and Rz. Let's go back to our last accelerometer model and do some additional notations:

The angles that we are interested in are the angles between X,Y,Z axes and the force vector R. We'll define these angles as Axr, Ayr, Azr. You can notice from the right-angle triangle formed by R and Rx that:

cos(Axr) = Rx / R , and similarly :
cos(Ayr) = Ry / R
cos(Azr) = Rz / R

We can deduct from Eq.1 that R = SQRT( Rx^2 + Ry^2 + Rz^2).

We can find now our angles by using arccos() function (the inverse cos() function ):

Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)

We've gone a long way to explain the accelerometer model, just to come up to these formulas. Depending on your applications you might want to use any intermediate formulas that we have derived. We'll also introduce the gyroscope model soon, and we'll see how accelerometer and gyroscope data can be combined to provide even more accurate inclination estimations.

But before we do that let's do some more useful notations:

cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R

This triplet is often called Direction Cosine , and it basically represents the unit vector (vector with length 1) that has same direction as our R vector. You can easily verify that:

SQRT(cosX^2 + cosY^2 + cosZ^2) = 1

This is a nice property since it absolve us from monitoring the modulus(length) of R vector. Often times if we're just interested in direction of our inertial vector, it makes sense to normalize it's modulus in order to simplify other calculations.

Part 2. Gyroscope

We're not going to introduce any equivalent box model for the gyroscope like we did for accelerometer, instead we're going to jump straight to the second accelerometer model and we'll show what does the gyroscope measure according to this model.

Each gyroscope channel measures the rotation around one of the axes. For instance a 2-axes gyroscope will measure the rotation around (or some may say "about") the X and Y axes. To express this rotation in numbers let's do some notations. First let's define:

Rxz – is the projection of the inertial force vector R on the XZ plane
Ryz – is the projection of the inertial force vector R on the YZ plane

From the right-angle triangle formed by Rxz and Rz, using Pythagorean theorem we get:

Rxz^2 = Rx^2 + Rz^2 , and similarly:
Ryz^2 = Ry^2 + Rz^2

also note that:

R^2 = Rxz^2 + Ry^2 , this can be derived from Eq.1 and above equations, or it can be derived from right-angle triangle formed by R and Ryz
R^2 = Ryz^2 + Rx^2

We're not going to use these formulas in this article but it is useful to note the relation between all the values in our model.

Instead we're going to define the angle between the Z axis and Rxz, Ryz vectors as follows:

Axz – is the angle between the Rxz (projection of R on XZ plane) and Z axis
Ayz – is the angle between the Ryz (projection of R on YZ plane) and Z axis

Now we're getting closer to what the gyroscope measures. Gyroscope measures the rate of changes of the angles defined above. In other words it will output a value that is linearly related to the rate of change of these angles. To explain this let's assume that we have measured the rotation angle around axis Y (that would be Axz angle) at time t0, and we define it as Axz0, next we measured this angle at a later time t1 and it was Axz1. The rate of change will be calculated as follows:

RateAxz = (Axz1 – Axz0) / (t1 – t0).

If we express Axz in degrees, and time in seconds , then this value will be expressed in deg/s . This is what a gyroscope measures.

In practice a gyroscope(unless it is a special digital gyroscope) will rarely give you a value expressed in deg/s. Same as for accelerometer you'll get an ADC value that you'll need to convert to deg/s using a formula similar toEq. 2 that we have defined for accelerometer. Let's introduce the ADC to deg/s conversion formula for gyroscope (we assume we're using a 10bit ADC module , for 8bit ADC replace 1023 with 255, for 12bit ADC replace 1023 with 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity Eq.3
RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

AdcGyroXZ, AdcGyroYZ – are obtained from our adc module and they represent the channels that measure the rotation of projection of R vector in XZ respectively in YZ planes, which is the equivalent to saying rotation was done around Y and X axes respectively.

Vref – is the ADC reference voltage we'll use 3.3V in the example below

VzeroRate – is the zero-rate voltage, in other words the voltage that the gyroscope outputs when it is not subject to any rotation, for the Acc_Gyro board it is for example 1.23V (you can find this values in the specs – but don't trust the specs most gyros will suffer slight offset after being soldered so measure VzeroRate for each axis output using a voltmeter, usually this value will not change over time once the gyro was soldered, if it variates – write a calibration routine to measure it before device start-up, user must be instructed to keep device in still position upon start-up for gyros to calibrate).

Sensitivity – is the sensitivity of your gyroscope it is expressed in mV / (deg / s) often written as mV/deg/s , it basically tells you how many mV will the gyroscope output increase , if you increase the rotation speed by one deg/s. The sensitivity of Acc_Gyro board is for example 2mV/deg/s or 0.002V/deg/s

Let's take an example, suppose our ADC module returned following values:

AdcGyroXZ = 571
AdcGyroXZ = 323

Using the above formula, and using the specs parameters of Acc_Gyro board we'll get:

RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s

In other words the device rotates around the Y axis (or we can say it rotates in XZ plane) with a speed of 306 deg/s and around the X axis (or we can say it rotates in YZ plane) with a speed of -94 deg/s. Please note that the negative sign means that the device rotates in the opposite direction from the conventional positive direction. By convention one direction of rotation is positive. A good gyroscope specification sheet will show you which direction is positive, otherwise you'll have to find it by experimenting with the device and noting which direction of rotation results in increasing voltage on the output pin. This is best done using an oscilloscope since as soon as you stop the rotation the voltage will drop back to the zero-rate level. If you're using a multimeter you'd have to maintain a constant rotation rate for at least few seconds and note the voltage during this rotation, then compare it with the zero-rate voltage. If it is greater than the zero-rate voltage it means that direction of rotation is positive.

Part 3. Putting it all together. Combining accelerometer and gyroscope data.

If you're reading this article you probably acquired or are planning to acquire a IMU device, or probably you're planning to build one from separate accelerometer and gyroscope devices.

NOTE: FOR PRACTICAL IMPLEMENTATION AND TESTING OF THIS ALGORITHM PLEASE READ THIS ARTICLE:

http://starlino.com/imu_kalman_arduino.html

The first step in using a combination IMU device that combines an accelerometer and a gyroscope is to align their coordinate systems. The easiest way to do it is to choose the coordinate system of accelerometer as your reference coordinate system. Most accelerometer data sheets will display the direction of X,Y,Z axes relative to the image of the physical chip or device. For example here are the directions of X,Y,Z axes as shown in specifications for the Acc_Gyro board:

Next steps are:

- identify the gyroscope outputs that correspond to RateAxz , RateAyz values discussed above.
- determine if these outputs need to be inverted due to physical position of gyroscope relative to the accelerometer

Do not assume that if a gyroscope has an output marked X or Y, it will correspond to any axis in the accelerometer coordinate system, even if this output is part of an IMU unit. The best way is to test it.

Here is a sample sequence to determine which output of gyroscope corresponds to RateAxz value discussed above.

- start from placing the device in horizontal position. Both X and Y outputs of accelerometer would output the zero-g voltage (for example for Acc_Gyro board this is 1.65V)
- next start rotating the device around the Y axis, another way to say it is that you rotate the device in XZ plane, so that X and Z accelerometer outputs change and Y output remains constant.
- while rotating the device at a constant speed note which gyroscope output changes, the other gyroscope outputs should remain constant
- the gyroscope output that changed during the rotation around Y axis (rotation in XZ plane) will provide the input value for AdcGyroXZ, from which we calculate RateAxz
- the final step is to ensure the rotation direction corresponds to our model, in some cases you may have to invert the RateAxz value due to physical position of gyroscope relative to the accelerometer
- perform again the above test, rotating the device around the Y axis, this time monitor the X output of accelerometer (AdcRx in our model). If AdcRx grows (the first 90 degrees of rotation from horizontal position), then AdcGyroXZ should decrease. This is due to the fact that we are monitoring the gravitation vector and when device rotates in one direction the vector will rotate in oposite direction (relative to the device coordonate system, which we are using). So, otherwise you need to invert RateAxz , you can achieve this by introducing a sign factor in Eq.3, as follows:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity , where InvertAxz is 1 or -1

same test can be done for RateAyz , by rotating the device around the X axis, and you can identify which gyroscope output corresponds to RateAyz, and if it needs to be inverted. Once you have the value for InvertAyz, you should use the following formula to calculate RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

If you would do these tests on Acc_Gyro board you would get following results:

- the output pin for RateAxz is GX4 and InvertAxz = 1
- the output pin for RateAyz is GY4 and InvertAyz = 1

From this point on we'll consider that you have setup your IMU in such a way that you can calculate correct values for Axr, Ayr, Azr (as defined Part 1. Accelerometer) and RateAxz, RateAyz (as defined in Part 2. Gyroscope). Next we'll analyze the relations between these values that turn out useful in obtaining more accurate estimation of the inclination of the device relative to the ground plane.

You might be asking yourself by this point, if accelerometer model already gave us inclination angles of Axr,Ayr,Azr why would we want to bother with the gyroscope data ? The answer is simple: accelerometer data can't always be trusted 100%. There are several reason, remember that accelerometer measures inertial force, such a force can be caused by gravitation (and ideally only by gravitation), but it might also be caused by acceleration (movement) of the device. As a result even if accelerometer is in a relatively stable state, it is still very sensitive to vibration and mechanical noise in general. This is the main reason why most IMU systems use a gyroscope to smooth out any accelerometer errors. But how is this done ? And is the gyroscope free from noise ?

The gyroscope is not free from noise however because it measures rotation it is less sensitive to linear mechanical movements, the type of noise that accelerometer suffers from, however gyroscopes have other types of problems like for example drift (not coming back to zero-rate value when rotation stops). Nevertheless by averaging data that comes from accelerometer and gyroscope we can obtain a relatively better estimate of current device inclination than we would obtain by using the accelerometer data alone.

In the next steps I will introduce an algorithm that was inspired by some ideas used in Kalman filter, however it is by far more simple and easier to implement on embedded devices. Before that let's see first what we want our algorithm to calculate. Well , it is the direction of gravitation force vector R = [Rx,Ry,Rz] from which we can derive other values like Axr,Ayr,Azr or cosX,cosY,cosZ that will give us an idea about the inclination of our device relative to the ground plane, we discuss the relation between these values in Part 1. One might say – don't we already have these values Rx, Ry , Rz from Eq.2 in Part 1 ? Well yes, but remember that these values are derived from accelerometer data only, so if you would be to use them directly in your application you might get more noise than your application can tolerate. To avoid further confusion let's re-define the accelerometer measurements as follows:

Racc – is the inertial force vector as measured by accelerometer, that consists of following components (projections on X,Y,Z axes):

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

So far we have a set of measured values that we can obtain purely from accelerometer ADC values. We'll call this set of data a "vector" and we'll use the following notation.

Racc = [RxAcc,RyAcc,RzAcc]

Because these components of Racc can be obtained from accelerometer data , we can consider it an input to our algorithm.

Please note that because Racc measures the gravitation force you'll be correct if you assume that the length of this vector defined as follows is equal or close to 1g.

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),

However to be sure it makes sense to update this vector as follows:

This will ensure the length of your normalized Racc vector is always 1.

Next we'll introduce a new vector and we'll call it

Rest = [RxEst,RyEst,RzEst]

This will be the output of our algorithm , these are corrected values based on gyroscope data and based on past estimated data.

Here is what our algorithm will do:
- accelerometer tells us: "You are now at position Racc"
- we say "Thank you, but let me check",
- then correct this information with gyroscope data as well as with past Rest data and we output a new estimated vector Rest.
- we consider Rest to be our "best bet" as to the current position of the device.

Let's see how we can make it work.

We'll start our sequence by trusting our accelerometer and assigning:

Rest(0) = Racc(0)

By the way remember Rest and Racc are vectors , so the above equation is just a simple way to write 3 sets of equations, and avoid repetition:

RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)

Next we'll do regular measurements at equal time intervals of T seconds, and we'll obtain new measurements that we'll define as Racc(1), Racc(2) , Racc(3) and so on. We'll also issue new estimates at each time intervals Rest(1), Rest(2), Rest(3) and so on.

Suppose we're at step n. We have two known sets of values that we'd like to use:

Rest(n-1) – our previous estimate, with Rest(0) = Racc(0)
Racc(n) – our current accelerometer measurement

Before we can calculate Rest(n) , let's introduce a new measured value, that we can obtain from our gyroscope and a previous estimate.

We'll call it Rgyro , and it is also a vector consisting of 3 components:

Rgyro = [RxGyro,RyGyro,RzGyro]

We'll calculate this vector one component at a time. We'll start with RxGyro.

Let's start by observing the following relation in our gyroscope model, from the right-angle triangle formed by Rz and Rxz we can derive that:

tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)

Atan2 might be a function you never used before, it is similar to atan, except it returns values in range of (-PI,PI) as opposed to (-PI/2,PI/2) as returned by atan, and it takes 2 arguments instead of one. It allows us to convert the two values of Rx,Rz to angles in the full range of 360 degrees (-PI to PI). You can read more about atan2 here.

So knowing RxEst(n-1) , and RzEst(n-1) we can find:

Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).

Remember that gyroscope measures the rate of change of the Axz angle. So we can estimate the new angle Axz(n) as follows:

Axz(n) = Axz(n-1) + RateAxz(n) * T

Remember that RateAxz can be obtained from our gyroscope ADC readings. A more precise formula can use an average rotation rate calculated as follows:

RateAxzAvg = ( RateAxz(n) + RateAxz(n-1) ) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T

The same way we can find:

Ayz(n) = Ayz(n-1) + RateAyz(n) * T

Ok so now we have Axz(n) and Ayz(n). Where do we go from here to deduct RxGyro/RyGyro ? From Eq. 1 we can write the length of vector Rgyro as follows:

|Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

Also because we normalized our Racc vector, we may assume that it's length is 1 and it hasn't changed after the rotation, so it is relatively safe to write:

|Rgyro| = 1

Let's adopt a temporary shorter notation for the calculations below:

x =RxGyro , y=RyGyro, z=RzGyro

Using the relations above we can write:

x = x / 1 = x / SQRT(x^2+y^2+z^2)

Let's divide numerator and denominator of fraction by SQRT(x^2 + z^2)

x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )

Note that x / SQRT(x^2 + z^2) = sin(Axz), so:

x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )

Now multiply numerator and denominator of fraction inside SQRT by z^2

x = sin(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)) )

Note that z / SQRT(x^2 + z^2) = cos(Axz) and y / z = tan(Ayz), so finally:

x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )

Going back to our notation we get:

RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )

same way we find that

RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )

Side Note: it is possible to further simplify this formula. By dividing both parts of the fraction by sin(Axz(n)) you get:
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cos(Axz(n))^2 / sin(Axz(n))^2 * tan(Ayz(n))^2 )
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 )
now add and substract     cos(Axz(n))^2/sin(Axz(n))^2   = cot(Axz(n))^2
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2  - cos(Axz(n))^2/sin(Axz(n))^2   + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2  + cot(Axz(n))^2 )
and by grouping terms 1&2 and then 3&4 we get
RxGyro = 1 / SQRT (1 + cot(Axz(n))^2 * sec(Ayz(n))^2 ),    where cot(x) = 1 / tan(x) and sec(x) = 1 / cos(x)
This formula uses only 2 trigonometric functions and can be computationally less expensive. If you have Mathematica program you can verify itby evaluating    FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]

Now, finally we can find:

RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).

Where Sign(RzGyro) = 1 when RzGyro>=0 , and Sign(RzGyro) = -1 when RzGyro<0.

One simple way to estimate this is to take:

Sign(RzGyro) = Sign(RzEst(n-1))

In practice be careful when RzEst(n-1) is close to 0. You may skip the gyro phase altogether in this case and assign: Rgyro = Rest(n-1). Rz is used as a reference for calculating Axz and Ayz angles and when it's close to 0, values may overflow and trigger bad results. You'll be in domain of large floating point numbers where tan() / atan() function implementations may lack precision.

So let's recap what we have so far, we are at step n of our algorithm and we have calculated the following values:

Racc – current readings from our accelerometer
Rgyro – obtained from Rest(n-1) and current gyroscope readings

Which values do we use to calculate the updated estimate Rest(n) ? You probably guessed that we'll use both. We'll use a weighted average, so that:

Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)

We can simplify this formula by dividing both numerator and denominator of the fraction by w1.

Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)

and after substituting w2/w1 = wGyro we get:

Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)

In the above formula wGyro tells us how much we trust our gyro compared to our accelerometer. This value can be chosen experimentally usually values between 5..20 will trigger good results.

The main difference of this algorithm from Kalman filter is that this weight is relatively fixed , whereas in Kalman filter the weights are permanently updated based on the measured noise of the accelerometer readings. Kalman filter is focused at giving you "the best" theoretical results, whereas this algorithm can give you results "good enough" for your practical application. You can implement an algorithm that adjusts wGyro depending on some noise factors that you measure, but fixed values will work well for most applications.

We are one step away from getting our updated estimated values:

RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)

Now let's normalize this vector again:

R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2 )

RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R
RzEst(n) = RzEst(n)/R

And we're ready to repeat our loop again.

Kalman filter vs Complementary filter

2013-04-14T17:54:00.000+10:00

(source: http://robottini.altervista.org/tag/complementary-filter )

The use of accelerometer and gyroscope to build autonomous robots, such as the self-balancing, requires a math filter in order to merge the signals returned by the sensors.
The gyroscope has a drift and in a few time the values returned are completely wrong. The accelerometer, from the other side, returns a true value when the acceleration is progressive but it suffers much the vibrations, returning values of the angle wrong.

Usually a math filter is used to mix and merge the two values, in order to have a correct value: the Kalman filter . This is the best filter you can use, even from a theoretical point of view, since it is one that minimizes the errors from the true signal value. However it is very difficult to understand. In fact, the filter needs to be able to calculate the coefficients of the matrices, the process-based error, measurement error, etc. that are not trivial.

In the hobbistic world, recently are emerging other filters, called complementary filters. In fact, they manage both high-pass and low-pass filters simultaneously. The low pass filter filters high frequency signals (such as the accelerometer in the case of vibration) and high pass filters that filter low frequency signals (such as the drift of the gyroscope). By combining these filters, you get a good signal, without the complications of the Kalman filter. (side note regarding sensor fusion: Accelerometer requires low pass filter, Gyro requires high pass filter, Vision require low pass filter)

Making a study from a theoretical point of view, the discussion is complicated and is beyond the scope of this tutorial. The complementary filters can be have different ‘orders’. Here I speak about the so-called first-order filter that filter already well, and the second-order filter which filter even better. Clearly, going from first to second order, the algorithm is more complicated to use and perhaps there is no gain so obvious to justify the increase in complexity.
A great introduction to the first order complementary filters applied to the an accelerometer and a gyroscope, comes from MIT (here). It introduces the filter in a a very simple mode. On this document is based the first Arduino algorithm:

// a=tau / (tau + loop time)
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()

float tau=0.075;
float a=0.0;

float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}

It ’enough to choose the response time of tau (please see here for Tau selection process ), to send the arguments, ie the angle measured with the accelerometer and the gyroscope, the time of the loop and you get in two lines, the angle calculated by the filter.

The algorithm at the base of the second order complementary filter is described here. Indeed it is not described at all, but now we’ve figured out how the filter works by the MIT’s documentation. The principle is the same, the algorithm is more complicated. The translation of this algorithm for the Arduino:

// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()

float Complementary2(float newAngle, float newRate,int looptime) {
float k=10;
float dtc2=float(looptime)/1000.0;

x1 = (newAngle -   x_angle2C)*k*k;
y1 = dtc2*x1 + y1;
x2 = y1 + (newAngle -   x_angle2C)*2*k + newRate;
x_angle2C = dtc2*x2 + x_angle2C;

return x_angle2C;
}

Here too we just have to set the k and magically we get the angle.

If we want to apply the Kalman filter, we can re-use one of the codes already present in internet. This is the code that I copied from the Arduino forum (here):

// KasBot V1 - Kalman filter module

float Q_angle  =  0.01; //0.001
float Q_gyro   =  0.0003;  //0.003
float R_angle  =  0.01;  //0.03

float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float  y, S;
float K_0, K_1;

// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()

float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}

To get the answer, you have to set 3 parameters: Q_angle, R_angle,R_gyro. The activity is a bit complicated .
But what happens with these algorithms? Similar curves are obtained? Here’s a comparison:
There are 5 curves:

Color lines:

Red - accelerometer
Green - Gyro
Blue - Kalman filter
Black - complementary filter
Yellow - the second order complementary filter

As you can see the signals filtered are very similarly. Note that in the presence of vibrations, the accelerometer (red) generally go crazy. The gyro (green) has a very strong drift increasing int the time.

Now let’s see a comparison only between a filtered signal. That kalman (green), complementary (black) and complementary second-order (yellow). You can see how the Kalman is a bit late vs complementary filters, but it is more responsive to the vibration. In this case the second order filter does not return an ideal curve, probably I have to work a bit on the coefficients.

In conclusion I think that the complementary filter, in this case the first order, can be used in place of the Kalman filter. The smoothing is good and the algorithm is much simpler than Kalman.

Turtlebot Navigation Report and Code

2013-03-18T21:27:00.000+11:00

One of the group in my robotics class (group consist of Zongyuan Ge, Yonhon Ng, Yonsen Ng, Lu Xia

and Yuantao Zhang)

Abstract:
In this project, we are given the task of implementing algorithm for our Turtlebot (Ted) to escape from an enclosure and to detect several objects after successfully escaped. We have implemented a maximum distance finding algorithm which coordinates with an obstacle avoidance module which works at
the same time to find the exit. Then we bridged OpenCV into our code and used SURF to detect key points in the image and match them with pictures in the library. Though we have encountered many difficulties, the project is finally accomplished and tested.

Report:
https://docs.google.com/file/d/0B8gYxW2umHiVSlI0LXZjdnFjT0U/edit?usp=sharing

Code:
https://docs.google.com/file/d/0B8gYxW2umHiVRVd1WHZHQjNIeWM/edit?usp=sharing

Adjacency/Incidence Matrix

2012-04-27T14:30:00.001+10:00

Adjacency Matrix

An adjacency matrix is a means of representing which vertices (or nodes) of a graph are adjacent to which other vertices.

Incidence Matrix

The incidence matrix of the undirected graph is a matrix consisting of 4 rows (corresponding to the four vertices, 1-4) and 4 columns (corresponding to the four edges, e1-e4):

Aileron, Rudder and Elevator (Pictorial Description)

2012-04-13T16:44:00.000+10:00

Aileron

The ailerons are used to control the aircraft in roll, which results in a change in heading due to the tilting of the lift vector. The two ailerons are typically interconnected so that one goes down when the other goes up: the down-going aileron increases the lift on its wing while the up-going aileron reduces the lift on its wing, producing a rolling moment about the aircraft's longitudinal axis. Ailerons are usually situated near the wing tip, but may sometimes be situated nearer the wing root. The terms "outboard aileron" and "inboard aileron" are used to describe these positions respectively. The word aileron is French for "little wing".

Rudder

A rudder is a device used to steer a ship, boat, submarine, hovercraft, aircraft or other conveyance that moves through a medium (generally air or water). On an aircraft the rudder is used primarily to counter adverse yaw and p-factor and is not the primary control used to turn the airplane. A rudder operates by redirecting the fluid past the hull or fuselage, thus imparting a turning or yawing motion to the craft.

Elevator

Elevators are flight control surfaces, usually at the rear of an aircraft, which control the aircraft's orientation by changing the pitch of the aircraft, and so also the angle of attack of the wing. In simplified terms, they make the aircraft nose-up or nose-down. (Ascending and descending are more a function of the wing—aircraft typically land nose up.) An increased wing angle of attack will cause a greater lift to be produced by the profile of the wing, and a slowing of the aircraft speed. A decrease in angle of attack will produce an increase in speed. The elevators may be the only pitch control surface present (and are then called a slab elevator or stabilator), or may be hinged to a fixed or adjustable surface called a tailplane or horizontal stabilizer.

Glossary of RC Helicopters

2012-04-11T14:13:00.001+10:00

Introduction

In this guide i will use the T-Rex 500 as the example, to guide you through the various components of the helicopter. I am not however recommending this as a beginners model. Under one-on-one tuition it is possible to learn on this aircraft, and in fact its flight characteristic can be set to be very docile and beginner friendly. But it is aimed more towards the mid to high end range and competition flight, they are also many times more expensive than a basic ready to fly box set, that a beginner should be aiming for.

Helicopter Mechanics

Airframe

Usually injection moulded. But also on more expensive models constructed from carbon fibre, and Aluminium. Providing a rigid frame to piece all of the components together, and transfer the power between the motor and rotor head. It also serves as a frame to hold the electronics. Modern pod and boom sport models, are semi exposed except for a fiberglass painted canopy that covers the first half of the airframe via some simple retaining clips. This aids in orientation of the aircraft whilst in flight, and helps to improve aerodynamics.

Tail

Between the tail unit and main frame is the tail boom, it provides a route for the tail drive system and control linkages. It is necessary to have the rear rotor blades at a larger diameter than the main rotor blades away from the main frame. Usually there are also two support structures coming from the base of the helicopter frame, to a fixed point along the boom to aid structural rigidity. This is also where the horizontal stabilizer fins are located, a bit further towards the rear we find the vertical stabilizer connected to the tail drive unit. The power is normally transferred to the tail from the main motor via a belt drive, but it is also possible to have torque tube drives, that have a fixed drive shaft powering it. The rudder control on the radio controls the yaw of the helicopter via changes in pitch of the tail blades.

Swashplate

This is one of the most important parts of a RC helicopter, it is how the movements from the servos are transferred to the rotor blades, and finally translated into a movement of the helicopter in the direction required. It allows the stationary helicopter frame and servos to apply movement to the spinning blades pitch. In order to produce propulsion in a certain direction, as the blades spin, at certain points along the circumference of the rotation, the pitch of the blade changes, thereby creating more lift on the left side than the right for example. This will then cause the helicopter to move to the right. This is the Aileron control on the radio, and produces a sideways banking motion. It is the same theory for the Elevator control, it rocks the swashplate forwards and backwards, like the Aileron moves it from side to side.

Rotor-head

Relatively simple compared to the swashplate, as most of the control mixing has been done there. The main components here are the blade grips, they are able to rotate to allow for the changes in pitch of the individual blade. Also on the rotor head is the flybar, this is to provide a mechanical form of stabilization to the inherently unstable aircraft. To provide control to the rotor head, we have a complex mixture of washout arms, bell crank arms, mixer arms, pitch control arms and flybar control arms. All working together to keep the helicopter in the air. I wont go into detail on these in this section, as the particular setup varies from one helicopter to another. I will however go into more detail in the mechanical setup section of this website.

A combination of rotor pitch and head speed gives the helicopter lift. When applying increasing pitch on both blades at the same time, this will produce lift, but also increased drag effects will reduce the head speed. On a modern transmitter, the throttle, and pitch are mixed to counter act this and keep a constant head speed, this is also where a engine governer comes into force on a nitro model.

To calculate head speed, we look at the rating of the motor, so a 3000 Kv motor will rotate at 3000 rpm per volt. So the T-Rex 500 runs at 22.2 v. This means the motor will spin at 66,600 rpm when the full voltage is applied, it then makes sense that at half throttle, we will have 33,300 rpm. It is then a simple case of working out the ratio between the teeth on the motor pinion and main gear to give us our head speed. So as an example, if for every one turn of the main gear, the motor turns 40 times, we would have a head speed of 1665 rpm.

Speed Controller and Motor

Unlike a nitro model where we have a mechanical mixture control and servo, on a electric helicopter we have a brushless motor and electronic speed controller [ESC]. The speed controller acts as many units in one. Supplying the appropriate voltage to the motor, providing power to the electronics, controlling headspeed, battery level warnings, and many more features. Most ESC's will have a built in battery eliminator circuit (BEC) to maintain the correct voltages to the receiver and ultimately the servos and gyro. If you have a nitro model, this will be a separate unit, and the helicopter electronics will have there own, smaller Li-Po batter pack. The ESC usually will connect to the motor via 3 cables, positive, negative and data. it is important when working on your helicopter on the ground, to disconnect the motor from the speed controller, to avoid accidental arming and spooling up.

Servo's and Gyro

If you have a 120 degree swashplate you will have 3 servos mixed to control the swashplate movement. This is known as eCCPM, the controls are mixed on the transmitter, to allow the three servos to control the four directions of motion applied on the cyclic control. Some helicopters use 90 degree non-ccpm swashplates, but on the majority of current helicopters these are much less common. A quick visual inspection of your swashplate will allow you to identify easily which type you have.

Lastly we have the tail servo, usually attached directly to the tail boom, or tucked away inside the main frame below were the tail boom interfaces with the frame. This provides the control for the pitch of the rear tail blades, to allow the model to yaw and rudder control. However, it is necessary to incorporate a gyro between the receiver and servo, to control the position of the tail. Due to the force of the tail rotor, it will generally always want to move. Also looking at the laws of motion, once moving it will want to continue on that path until operated on by an external force. So a gyro counteracts this. It keeps the tail locked steady in one position, when you give some left rudder, it moves left, but the moment you stop the control input the tail comes to sharp stop. Without this you would have to be constantly balancing the tail, and the other controls.

Helicopters

2012-04-11T14:08:00.001+10:00

Helicopters have very complex mechanics. They contain many tens of thousands of moving parts. The basic principles of how they work can be grasped relatively quickly though. A horizontal power driven rotor provides lift to a helicopter, more commonly known as the main rotor. This produces a reaction torque. This is counteracted by the vertical tail rotor.

Main Rotor

Helicopter lift is generated by the main rotor. The rotor blades spin, cutting through the air. Sucking it in from above and directing it downwards to produce thrust. Each blade must be able to hold the weight of the helicopter equally. So if the helicopter was to weigh 2000 kg, each blade needs to be able to support 1000 kg. Multiplied by the respective gravitational factor. Depending on the specific style of flight. Pulling a tight turn, generating 2G, would mean the helicopter now effectively weighs 4000kg.

Tail Rotor

The tail rotor is very important, when the main rotor spins, it causes a torque reaction. The helicopter will want to spin in the opposite direction to the blades. A rear tail rotor will counteract this, also known as a anti-torque unit. Derived directly from newtons third law, the tail rotor will cancel out the torque generated by the main rotor.

Another technology is NOTAR, NO TAil Rotor. This directs a jet of air down the tail shaft, and mechanics at the end of the boom divert it to the required direction to keep the helicopter stationary. One main advantage is how silent the system is, making for a very quiet helicopter.

How Servo Works!

2012-04-11T13:59:00.000+10:00

Servos are controlled by sending them a pulse of variable width. The control wire is used to send this pulse. The parameters for this pulse are that it has a minimum pulse, a maximum pulse, and a repetition rate. Given the rotation constraints of the servo, neutral is defined to be the position where the servo has exactly the same amount of potential rotation in the clockwise direction as it does in the counter clockwise direction. It is important to note that different servos will have different constraints on their rotation but they all have a neutral position, and that position is always around 1.5 milliseconds (ms).

The angle is determined by the duration of a pulse that is applied to the control wire. This is called Pulse width Modulation. The servo expects to see a pulse every 20 ms. The length of the pulse will determine how far the motor turns. For example, a 1.5 ms pulse will make the motor turn to the 90 degree position (neutral position).

When these servos are commanded to move they will move to the position and hold that position. If an external force pushes against the servo while the servo is holding a position, the servo will resist from moving out of that position. The maximum amount of force the servo can exert is the torque rating of the servo. Servos will not hold their position forever though; the position pulse must be repeated to instruct the servo to stay in position.

When a pulse is sent to a servo that is less than 1.5 ms the servo rotates to a position and holds its output shaft some number of degrees counterclockwise from the neutral point. When the pulse is wider than 1.5 ms the opposite occurs. The minimal width and the maximum width of pulse that will command the servo to turn to a valid position are functions of each servo. Different brands, and even different servos of the same brand, will have different maximum and minimums. Generally the minimum pulse will be about 1 ms wide and the maximum pulse will be 2 ms wide.

Another parameter that varies from servo to servo is the turn rate. This is the time it takes from the servo to change from one position to another. The worst case turning time is when the servo is holding at the minimum rotation and it is commanded to go to maximum rotation. This can take several seconds on very high torque servos.

LiPo Batteries Guide and Regulator

2012-04-10T13:31:00.000+10:00

What is a LiPo?
LiPo or Lithium Polymer is a type of battery chemistry. These batteries are excellent for use in an application such as remote control aircraft due to their high energy to weight ratio. However, they do require a little special care to make sure that they always work as expected and to minimise risk of battery failure.

LiPos can be dangerous if overcharged, or over-discharged. Lithium burns very quickly when exposed to air, so fast that it looks like the lithium is exploding.

Nominal or resting voltage - 3.7v/cell (volts per cell)
Charged or peak voltage - 4.2v/cell

Charged or peak voltage is the maximum voltage the battery pack will store.

Naming convention
A battery is a collection of cells. All batteries have a naming convention which is designed to help the user identify how many cells the battery is made of and in what configuration they are. every cell has a capacity (stored energy), measured in mAh (milliamp hours) and a voltage, measured in volts. Cells within a battery can be connected in series or parallel. Please note that all the cells must be exactly alike.

Series connection - add the voltages of each cell together while the capacity of the whole battery remains the same as for 1 cell.
Parallel connection - add the capacities of each cell together while the voltage of the whole battery remains the same as for 1 cell.

For example, lets say we have a bunch of 1000mAh 3.7v cells.

If we connect 3 cells in series, the battery would be a 11.1v (3.7v + 3.7v + 3.7v) 1000mAh battery.
If we connect 3 cells in parallel, the battery would be a 3.7v 3000mAh (1000mAh + 1000mAh + 1000mAh) battery.

Now, with regard to the naming convention, the battery is named according to the following: the voltage of the whole battery, the capacity in mAh, the number of cells in series and then number of cell series in the battery.

The above 2 examples would be named as follows:

3s1p 11.1v 1000mAh - a battery made up of 3x 1000mAh cells connected in series.
1s3p 3.7v 3000mAh - a battery made up of 3x 1000mAh cells connected in parallel.

A 14.8v 2200mAh 4s1p LiPo battery would mean that the battery is made up of 4x 2200mAh 3.7v LiPo cells connected in series.

C rating - what is it and what does it mean?
The C rating, or discharge rating of the pack is a very important concept to understand if you're going to be doing anything with LiPo batteries. A LiPo pack should have a C rating on the label, anywhere from 10C to 40C typically. The C rating is the maximum safe discharge current, measured in Amps, that the battery is capable of delivering. The C is simply the capacity of the pack, and the rating is given with a precursor number, ie 20C.

Lets take the battery pack above, the 14.8v 2200mAh 4s1p LiPo. If it was a 20C pack, then the maximum discharge in amps would be calculated as follows: 20x2200 = 44000mAh. To get the value in amps you divide the answer by 1000 as 1Amp = 1000milliAmps. 44000/1000 = 44Amps. This pack's maximum discharge current is 44A. Any setup which draws more than that current will risk damaging the battery.

As a general rule, at ChargedRC we always recommend that you set up your power system to pull no more than 80% of the max charge rate at full power. This leaves you with a bit of headroom just in case. If your pack's max discharge rate was 50A, aim for your setup to pull 40A at full power on the ground. If you need more amps (and we all do ;) ), consider a higher C rated battery pack, or a pack of larger capacity.

Charging
Please charge your LiPo battery only on a charger specifically designed for LiPo batteries. Those chargers will have a special LiPo charging mode. There may also be just a normal charge mode, as well as a balance charge mode. Please check your LiPo before you charge it. Look for signs of swelling of the individual cells, any discolouration or if any part of the pack is warm. If your pack has any of those characteristics, please DO NOT charge or discharge it.

Now that the naming convention has been described, please make sure of the following when charging:

When you plug your battery in to charge and begin charging, wait and make sure that the voltage on the display screen of the charger does not exceed whatever the pack voltage is. Ie if you have a 4cell (4s) pack, the voltage should not exceed 16.8v (4.2v x 4).
If the charger is capable of doing a balance charge (charging all the cells in the pack individually and at the same time), make sure that all the cells are around the same voltage
Never leave a charging battery unattended
Always charge your battery on a non-flammable surface or in a container specially designed for charging LiPo batteries

Charge Rate
A lot of people ask us what charge rate to use for their LiPo. Charging your LiPo at the correct rate is essential to ensuring the life and quality of the pack, as well as charging it as fast as possible, but within its limits. The answer though is very simple. Unless otherwise stated, a battery's charge rate is simply 1C.

If you had a 2200mAh battery, the charge rate would be 2.2A (2200/1000).
If you had a 5000mAh battery, the charge rate would be 5A.

We hope that this guide gives you a better understanding of LiPo batteries. If you have any questions, please jump on our forum, or contact us for more information.

I crashed :( Is my battery still OK?
Everyone crashes, we've all done it and it's a part of the hobby. If you happen to crash an aircraft which is powered with a LiPo, please approach the crash site carefully. Disconnect the battery from the ESC very carefully, and try not to touch the pack too much. If you can, move the pack somewhere out of the way and keep an eye on it for at least 30 minutes. Watch for swelling or softness on the battery surface and if any part of it heats up. As explained above, Lithium reacts very violently with air. The pack may have somehow been punctured in the crash. If the puncture is very small, it can take quite a while for air to work its way into the pack. If the pack is swelled or puffed DO NOT touch the pack directly. The battery is definitely damaged and you should follow the disposal instructions. Also, if the pack is deformed or damaged in any way PLEASE do not use it again and dispose of it carefully as per the instructions below.

LiPo battery disposal

In order to properly dispose of a LiPo battery, you must first discharge it as low as possible. Please, if the battery has been damaged DO NOT discharge it, just proceed with the next step
Fill a bucket or some plastic container (NO METAL) with water, and add salt. Add approximately 1/4 cup of salt for every litre of water. The container may have a lid but must not be air tight
Immerse the battery in the salt water fully. Make sure that even the power connectors are submerged. Leave the battery for at least 2 weeks.
After 2 weeks, remove the battery, wrap in newspaper and dispose of in rubbish bin like normal rubbish.

Battery balancing
Battery balancing and battery redistribution refer to techniques that maximize a battery's capacity to make all of its energy available for use and increase the battery's lifetime. Balancers are often found in Lithium ion battery packs. Typically, the individual cells in a battery have somewhat different capacities and may be at different levels of State Of Charge (SOC). Without redistribution, discharging must stop when the cell with the lowest capacity is empty (even though other cells are still not empty); this limits the energy that can be taken from and returned to the battery.

Without balancing, the cell of smallest capacity is a “weak point”, it can be easily overcharged or over-discharged while cells with higher capacity undergo only partial cycle. For the higher capacity cells to undergo full charge/discharge cycle of the largest amplitude, balancer should “protect” the weaker cells; so that in a balanced battery, the cell with the largest capacity can be filled without overcharging any other (i. e. weaker, smaller) cell, and it can be emptied without over-discharging any other cell. Battery balancing is done by transferring energy from or to individual cells, until the SOC of the cell with the lowest capacity is equal to the battery's SOC.

Battery Regulators

Sometimes we need voltage regulators with batteries (A voltage regulator is an electrical regulator designed to automatically maintain a constant voltage level). One of the useful battery regulator is Turnigy HV SBEC 5A switch regulator (input 8-42V, output 5V or 6V Select-able).

Align 600 -Servo Configuration (Aileron, Rudder, Elevator)

2012-04-10T12:31:00.001+10:00

Download the picture for pictorial description.

Servos Connection to Receiver (Spectrum AR7000) - DX7

2012-04-10T12:12:00.000+10:00

Many people get confused when trying to connect the Radio receiver in their electric powered helicopters, and this is quite understandable as the documentation is usually less than helpful to those that do not understood it all.

So this Tip and Trick will try to cut through the jargon and let you now how to work out the connections on most types of radios.

One of the first areas people don't understand is how there are TWO AILERON SERVOS. Well, one one side they are called AILERON, but the other side is not called that at all, it is commonly known as PITCH. However, very few RX have a PITCH connection, and you need to use the AUX1 connection.

Also, some RX units have names for the connections, others use numbers alone, which is even harder to understand.

So let's look first at a very common 6 channel receiver, the one provided by ESKY for the Belt CP. You can see below that it uses numbering, starting from 1 and going through to 6 with an additional B connection at the far end.

The Esky 6 channel Rx, which uses numbering to identify the channels. NB - MOST RX's use the same numbers to identify the same channels.
In this case we have Channels 1-6 + a B Channel

This is an older NEC 7 channel receiver, and you may have already noticed that it has a Batter channel at the left end, so yes, you were right, that is exactly what the "B" means on the Esky RX.
So, by looking at the NEC RX (above right), we can see quite easily what the various channels re for on BOTH RX's.

Channel 1
Channel 2
Channel 3
Channel 4
Channel 5
Channel 6
Channel 7
Channel B
Throttle
Aileron (NB - maybe the left or right servo)
Elevator
Rudder
GEAR (often used for gyro control in helicopters)
Auxiliary 1 (Used as Pitch, or in other words, the OTHER Aileron servo)
Auxiliary 2 (Spare for helicopters)
Battery - only used if you use an board 4.8V battery pack to power the Radio equipment

So, now you can hopefully see the correlation between numbers and actual words. Almost ALL RX's will use this same channel layout on their RX's.
Equally important to know about is the way around the connections should be made. The servo wires have either Red, Black and white wires, or Orange, Brown and yellow which corresponds to the previous wiring colour directly. In both cases, the white or yellow leads are the SIGNAL lead.

All connections to the RX MUST HAVE THE CONNECTIONS THE SAME WAY AROUND, so that the white lead is always the same side. Although not guaranteed, most RX's expect the white signal lead to be on the logical top of each connection

The Black and red wires are the power, and in fact, these two pins are connected right across the RX internally, so as long as you get the wire from the ESC the right way around, ALL other connectors will immediately have power on them.

So, as far as electric powered helicopters are concerned :

THROTTLE channel is where you plug the 3 wire lead in from the ESC (Electronic Speed Controller).

AILERON Channel is where one or other of the aileron servos are connected. This can vary, so it is often a matter of trial and error to see which is correct for which servo. If the swash plate moves the wrong way when you move the cyclic stick left and right, switch the connections between channel 2 and channel 6.

ELEVATOR Channel is where you plug in the 3 wire lead from the elevator servo

RUDDER Channel is where you plug in the 3 wire lead from the rudder servo, or more commonly, the lead from the gyro, and then connecting the rudder servo to the connector provided on the gyro so that the gyro is in circuit with the rudder servo.

GEAR Channel is of no real use to many helicopters, and is almost universally used to control the gain of the gyro, which switches it between RATE mode and HH (Heading Hold) modes. Quite often you will find this is just a single wire connection that comes from the gyro, and the wire must go into the signal channel in the RX. You can use a different channel on computer radios if you wish by programming your selection in the TX.

AUXILIARY 1 handles the OTHER AILERON Channel, together with Channel 1.

AUXILIARY 2 is a usually a spare Channel if you have it. Some expensive radios may have even more AUX Channels, but these are always programmed for use in the Radio TX.

BATTERY is rarely used with electric helicopters, as we use the main LIPO battery for all our power requirements via the ESC. However, those flying nitro and gasser helicopters will use this to provide the power to the radio system and servos as they will not have a Lipo or an ESC.

A headset that reads your brainwaves

2011-08-05T14:59:00.000+10:00

Tan Le, a Vietnamese-Australian telecommunications entrepreneur, is president and co-founder of Emotiv Systems , a neuro-engineering company that developed a breakthrough interface technology for digital media, taking inputs directly from the brain. Tan Le's vision is to revolutionize human-computer input in the same way the graphic user interface did 20 years ago

Based on the latest developments in neuro-technology, has developed a revolutionary new personal interface for human computer interaction. The Emotiv EPOC wireless neuro headset captures, understand and processes the thoughts in your brain to carry out actions on your PC. The real life application possibilities across industries like gaming, healthcare, security, market research etc has already got them partnerships with the who is who of research and investment world. Emotiv is also actively involving the developer community by offering them software kit to create applications that can be controlled by your mind.

Tan Le gives live demonstration of the headset at TEDx platform:

**Usman**