CA2241359A1 - Goniometer and method of use thereof - Google Patents

Abstract

An improved goniometer has an angular rate sensor and a computer for integrating the signal from the angular rate sensor to produce an angle signal. The angle signal is adjusted from time to time, manually or automatically to reduce drift from integration. For automatic adjustment, an accelerometer may be used to provide a reference signal with which the angle signal may be compared. The angle signal may be computed as a difference signal from two angular rate sensors.

Description

TITLE OF THE INVENTION
Goniometer and Method of Operation Thereof NAMES OF INVENTORS
Brian J. Andrews to Richard Williamson FIELD OF THE INVENTION
The invention relates to a method and apparatus for measuring rotation of a body and in particular, movement of joints, joint angless, relative and absolute inclinations, relative displacements, accelerations, and velocities.
BACKGROUND OF THE INVENTION
The motion of humans, animals, robots, and other objects with moving segments or appendages can be adequately described by kinematic values such as joint 2o angles, inclinations, displacements, accelerations, and velocities. The ability of a researcher to measure these values provides them with a better understanding of how living beings move, how they may be treated if motion disorders exist, how to provide and improve instantaneous feedback of motion, and how to map movements to robots, computers, and other forms of artificial intelligence in applications such as machine learning, virtual reality, and computer animation.
With respect to human motion, these types of measurements provide the backbone for the clinical analysis, treatment and fixrther scientific understanding of motion disorders ranging from movement restricted conditions such as cerebral palsy

2 or spinal cord injury to minor structural abnormalities such as stride length asymmetry.
Treatment of patients with motion abnormalities generally takes place in clinical settings where complex equipment is used to provide complete full body motion analysis. It is often the case, though, in which clinicians only require simple kinematic measurements without the use of such complete and expensive systems.
For to example, for a patient previously suffering from a stroke, walking velocity may be all that is needed to assess progress in recovery. Clinicians treating patients with knee injuries may only want to measure the angle of the knee joint in flexion in order to guide the patient as close as possible back to his or her normal range.
Further, these systems are often stationary, cumbersome, and don't offer the flexibility for use both in the clinic or laboratory and "normal" outside environments.
One of the main problems associated with present gait analysis systems or in the analysis of prosthetics is measurement inaccuracy due "whitecoat" walking.
Patients often consciously or unconsciously alter their true gait patterns while under the observing eye of the clinician or researcher. Greater accuracy in measurements of a 2o patients gait are possible outside of the clinic or laboratory when the patient is in a familiar setting without the pressure of a clinician watching i.e. walking around in the neighborhood or moving around the house. It is often advantageous for the clinician or researcher to be able to assess the use of prosthetics outside of the clinic. A
compact, unencumbering, robust kinematic measuring system would make this very easy to achieve.

3 Additionally, other applications such as the analysis of the movements used in sport depend upon the ability of the clinician or researcher to be able take measurements outside of the clinic or laboratory and in the activity of the sport being studied. For example, in a professional sport such as hockey where recovery from injury is of utmost importance to the player and the team, movement of the of injured 1o joints would be best measured in the player's "natural" environment, on the ice and in game situations.
Further to the idea of portability, is the use of simple kinematic measurement systems in conjunction with functional electric stimulation (FES) systems inside and outside of the laboratory. Measurements such as joint angles or relative positions in space are integral components for closed loop control of FES. For example, in the use of an FES system for climbing stairs, it would be useful to know the vertical height and position of the toe. A signal generated by kinematic measurement of this displacement could be used to avoid an impending collision with a step. In the case of a sit-stand-sit maneuver, the movement could be navigated properly having the 2o instantaneous knowledge of the pelvis, knee, and ankle inclinations, angles and displacements.
In general clinical rehabilitation, television/camera systems are used in which position markers, shiny retro reflectives, are mounted and positioned around on the body of the patient. Four cameras with infrared strobes view the markers on the body and with computer reconstruction can develop 3-dimensional models from the

4 markers. A force platform, which measures the forces between the foot and the floor in three dimensions, is set up with in the floor. The platform measures the vertical force before and after contact, the horizontal force, and the torques about those axes.
Full gait analysis can be performed knowing the position of the body and the forces with the foot to the floor.
1o This system is not without its problems, however. A setup of this sort is extremely expensive due to the magnitude and expense of the individual pieces of equipment involved. This can obviously become a financial burden if all one wanted to do is measure the simple kinematics of movement of a patient. Moreover, the size and complexity of this system limit it to only measurement taking within the clinic or laboratory.
A device that has tried to address some of these problems is the flexible goniometer, an electrogoniometer developed by Penny & Giles Ltd. The makeup of this device revolves around a strain gauged thin steel beam with each end attached to limb segments that articulate the joint of interest. Flexion of this beam produces a 2o voltage signal proportional to the angle between the joint segments.
Measurement accuracy is typically a tenth of a degree. However, flexion of the plastically stretching shim steel beam beyond a certain radius (approximately 1 cm), something which can easily happen, can permanently damage the device. This device is also expensive and has an encumbering mechanical linkage across the joint. It is limited in its application since it cannot be used to measure angles at other joints such as the hip joint. Contact

5 with clothing can disrupt measurements and the device is simply too fragile for everyday use with orthotic devices. Additionally, it requires a significant amount of power, making it unsuitable for use outside of the laboratory.

1o This invention provides a new goniometer, which has particular applicability to the detection of human body movement, but which may also be used on other animals and inanimate moving bodies. The invention provides a goniometer which includes an angular rate sensor, or gyroscope. A difficulty with the use of gyroscopes for the sensing of body movement is that integration of angular rate to yield an angle signal tends to accummulate errors with time. By adjusting the angle signal, these errors may be reduced.
A difficulty in the adjusting of angular rate sensors for determining body movement with greater accuracy is finding a suitable reference signal or datum to show how the errors are accummulating. One datum occurs when the body is 2o stationary. Whether a body is stationary may be determined by using an accelerometer.
In addition, an accelerometer with a DC response will provide a signal indicative of the angle of the body with respective to the earth's gravitational field. When the body is stationary, the accumulating angle signal may be reset to the angle of the body determined by the accelerometer.
Resetting of the angle signal may be triggered by a manual signal, or

6 automatically when a person's foot hits the ground, or at fixed repeated intervals, or by automatically detecting when a body is stationary and automatically resetting the angle sensor at that time.
An example of a manner of adjusting the angle signal is to provide a reference signal, which is itself preferably also an estimate of the angular position of the body 1o being sensed. An accelerometer, preferably co-located with the gyroscope, may provide the reference signal. The output from the accelerometer tends to contain more noise than the output from the gyroscope and therefore requires filtering.
This causes a delay in the reference signal compared with the angle signal, and the filtered reference signal may be compared with the angle signal in a computer to determine what adjustments need to be made to the angle signal to improve its accuracy.
Various adjustments may be made.
In a further aspect of the invention, an angle signal may also be computed from an integration of the difference between the outputs from two gyroscopes rotating in the same plane. In this case, the angle signal represents the angle between the two 2o gyroscopes. The differential angle signal may also be adjusted, for example by comparison to a reference signal.
These and other aspects of the invention will be now described, and claimed in the claims that follow the description.
BRIEF DESCRIPTION OF THE DRAWINGS
There will now be described preferred embodiments of the invention, with

7 reference to the drawings, by way of illustration only and not with the intention of limiting the scope of the invention, in which like numerals denote like elements and in which:
Fig. 1 is a schematic showing basic components for measuring joint angles with two gyroscopes, wherein the communications links between parts may be wired or wireless;
1o Fig. 2 is a schematic showing basic components for measuring joint angles as well as relative dispacements, relative inclinations, accelerations and velocities, wherein the communications links between parts may be wired or wireless ;
Fig. 3 is a graph showing comparison of relative joint angles computed from gyroscope signals (solid) and a Penny & Giles flexible goniometer;
Fig. 4 is a schematic representation of four linked segments;
Fig. 5 is a schematic showing lengths and angles in determining relative positions in space of knee and hip joints;
Fig. 6 shows stages of human gait;
Fig. 7 is a schematic showing inclination of a foot;
2o Fig. 8 is a schematic of the foot showing velocity components;
Fig. 9 is a schematic showing dual gyroscopes communicating with a computer by radio communication links;
Fig. 10 shows an exemplary use of the gyroscopes of Fig. 9;
Fig. 11 is a schematic showing components in an instrument cluster at the location of one of the gyroscopes of Fig. 10;
Fig. 12 shows an exemplary instrument location with gyroscopes and accelerometers co-located on a person's leg;
Fig. 13 shows components for an instrument cluster at the location of one of the accelerometers of Fig. 12;
3o Fig. 14 is a schematic showing relationship between the instrument clusters of Fig.
12 and a main computer unit;
Fig. 15 shows instrument clusters, accelerometers co-located with gyroscopes, on a

8 person's hand; and Fig. 16 is a graph showing angle variation with time for processed outputs g and a from a co-located gyroscope and accelerometer respectively, where the signal from the accelerometer is delayed by filtering..
to DESCRIPTION OF PREFERRED EMBODIMENTS OF THE INVENTION
The term gyroscope or angular rate sensor are used interchangeably in this patent document to describe a device that measures rate of change of orientation of a body. Gyroscopes that may be used in the carrying out of the invention include:
miniature gyroscopes which measure coriolis forces, including but not limited to spinning mass gyros, vibrating quartz rate gyro, fiber optic rate gyros, axial gyro, spinning magnetic fluid gyro, a surface micromachined gyro surface silicon micromachined gyro, microaxial gyros and all other miniature gyros. The gyro should be less than 1.5 inches in diameter and less than one gram in weight. The gyro can include a device containing an accelerometer or other components. For planar motion, 2o the gyro axis must be aligned perpendicularly to the plane of motion. An angular rate sensor may also be a device that measures rotational acceleration and integrates the rotational acceleration to obtain a rate of change of angle. A device that measures rotation acceleration is the ACH-04-08 accelerometer/shock sensor of AMP
Sensors (Internet address: amp.com).
The term "accelerometer" herein refers to devices that measure gravitational and inertical forces on an object. The measurement is made directly without

9 integration, and thus without accumulated errors. Miniature accelerometers are preferably used including but not limited to mechanical spinning mass, quartz vibrating structures, micromachined components and surface micromachined components, surface silicon micromachined component. If only one accelerometer is used then its axis must be aligned co-axial with the segment. Wherever a single gyroscope or to accelerometer is mentioned, it should be understood that more than one may be used.
The inventor has developed a new apparatus and method which utilizes gyros and accelerometers to take simple kinematic measurements including joint angles, relative and absolute displacements, relative inclinations, velocities, and accelerations of a set of linked limbs or segments.
For measuring the joint angle between two articulating segments, the basic elements of a gyro-goniometer are shown in Fig. 1, comprising of two gyros gl, g2 which generate signals representing angular velocities, one or more power sources 10 for providing the required power for operation of the gyros gl, g2, one or more analog-to-digital converters 12 for converting the analog gyro signals to digital signals, 2o and a computer/memory unit 14 which is configured to repeatedly carry out the integrations and other computations described below using the digital signals from the gyroscopes and thus to obtain the joint angle, and to store the measured joint angle to memory. The computer 14 or any of the other computer units described in this patent document may be any of various computers available in the art, and may be specially built to have the configuration required to carry out the functions described herein, or 5 may be an off the-shelf microprocessor configured by software to carry out the functions described herein. A manual reset 16 may also be provided for resetting integrators in the computer unit 14. In the case when the device illustrated in Fig. 1 is being used to control gait of a person, where the angle signal generated by the computer 14 is part of a feedback loop, the integration of the angular rate output from 1o the gyroscopes may be reset by the person pressing the manual reset. This reduces or prevents errors accumulating over time in the integration process. The angle signal computed by the computer 14 may be output to a conventional FES system 18 or other feedback system that controls the movement of the body being sensed. FES
systems are well known in themselves. For example, they are described in "Application of Tilt Sensors in Functional Electrical Stimulation, IEEE Trans. Rehab.Eng., vol. 4, no. 2, June 1996_ For measuring the relative inclinations, relative displacements, the velocities, and accelerations of a set of linked segments, the basic elements of a gyro-goniometer are shown in Fig. 2, comprising of one or more gyros gl, g2 ... gn which generate 2o signals representing angular velocities, one or more accelerometers al ...
an which generate signals representing acceleration and inclination, one or more power sources for providing the required power for operation of the gyros and accelerometers, one or more analog-to-digital converters 22 for converting the analog gyro and accelerometer signals to digital signals, and a computer/memory unit 24 to perform calculations on the digital signals to obtain the inclinations, displacements, velocities, and accelerations, and to store the measurements to memory. In this example, the angle signal computed by the computer 24 may be adjusted automatically and continuously using output from an accelerometer co-located with the gyroscopes, or an accelerometer that has a known angular orientation with respect to the gyroscopes.
In order to realize the flexible nature of this invention, the size and accuracy of to the gyros and accelerometers are an important consideration. The smaller the components get, the more versatile the invention becomes. Therefore, for the purpose of describing this invention as an apparatus that offers the most flexibility presently possible in terms of application, the gyros are preferably uni-axial miniature rate gyroscopes such as the Murata ENVOSA rate gyroscopes and the accelerometers are uni-axial micromachined accelerometers such as the Analog Devices ADXL,OS
accelerometers. These micromachined accelerometers have a DC response, a characteristic which enables the micromachined accelerometer to measure gravity, a static acceleration, and subsequently segment inclination.
The small size of these gyros and accelerometers lend well to portable and 2o compact applications without sacrificing accuracy as indicated in Fig. 3.
Using two Murata ENVOSA gyros in a motion simulating the movement of the knee during standing up (range 90 - 180 degrees) the gyro-goniometer response closely approximates the angle obtained with the Penny & Giles flexible goniometer.
The joint angle measured by the rate gyros (dashed) is superimposed on the solid line representing the flexible goniometer output to indicate a difference of less that 0.7 degrees RMS.
These gyros and accelerometers represent some of the latest technological advancements in terms of size, cost, and accuracy. However, as applications for this device vary and technology advances, the invention is not limited to the use of these gyros and accelerometers. The miniature rate gyro can be substituted for to micromachined rate gyros or any other types of uni or multi-axial gyros or devices which measure angular velocity. Similarly, the micromachined accelerometer with a DC response can be substituted for other types of uni or multi-axial accelerometers or devices which measure accelerations and/or inclinations.
The component arrangement and connections may be altered dependent upon the level of complexity of the motion to be analyzed or upon the financial resources available. Therefore, the best method of use or the optimum number of gyro or accelerometers for this invention will simply depend upon the application. In using a portable gyro-goniometer for clinical rehabilitation as an example, a compact portable computer/memory unit for use outside of the laboratory or clinic can easily be easily 2o substituted for a desktop PC if the analysis is to be done strictly in a lab or clinic.
Cables may replace any transmitter/receiver components for communicating the measured signals to the analog-to-digital converter and/or computer/memory unit.
The miniature portable analog-to-digital converters) can be substituted for a conventional analog-to-digital converters) which may or may not be part of the computer/memory unit. Though it is preferable for the integrator reset fi~nction to be automatic, manual reset as shown in Fig. 1 may be used. Two gyros may be all that is needed to analyze the motion of the leg whereas the hand may need as many as 20.
The components can follow a series or parallel arrangement dependant upon the complexity of motion and application.
The gyro-goniometer may also be used in conjunction with other devices or as 1o part of other devices with multiple applications. For example, it may be integrated into FES systems to enhance the functionality of the treatment by providing feedback to the stimulation controller...
Due to the variety of applications and configurations possible with the gyro-goniometer, a general method of use will be described through example. Using the schematic diagram of the four linked segments in Fig. 4, the description will involve the measurement of sit-stand-sit and normal gait motion in the saggital plane.
The segments, 1 through 4, will represent the pelvis, thigh, shank, and foot for one side of the body.
For complete functionality, the joint angle, segment inclinations, displacements, 2o velocities, and accelerations are desired. These values are generated by attaching a gyro gl-g4 and accelerometer al-a4 to each of the pelvis, thigh, shank, and foot. The accelerometer placed on each segment is used to establish initial inclinations of the segments with respect to the ground. For measurements of velocity and stride length (displacement), a second accelerometer is attached on any segment.
On-line resetting of the rate gyroscopes integral is used. This resetting algorithm takes the information provided by the accelerometer's calculation of and tilt of a segment, and correspond angular measurement, and compares it with the calculation of tilt from the integral of the rate gyroscope which is placed on the same limb segment as the accelerometer. If the angle as calculated from the rate gyroscope is drifting, due to the nature of the device, the rate gyroscope angle is reset to a value to calculated by the accelerometer. Since the signal provided by the accelerometers tends to have more noise than the signal provided by the gyroscopes, due to the nature of the devices, the output from the accelerometers is preferably filtered with a low pass or bandpass filter. The filter may be provided as part of the computer 14, 24 or 34, and may be a simple moving average. The filtering of the accelerometer output tends to delay the signal as shown in Fig. 16, so that for adjusting the angle signal generated by the computer 24, the adjustment value may be determined by comparing a past value of the signal calculated from the gyroscope output with the corresponding signal from the accelerometers. If the difference between the angle signal and the reference signal from the accelerometers exceeds a given level, for example 5 degrees, then an 2o adjustment is made. One adjustment is then to adjust the gyroscope output by the dii~erence between the corresponding past values of the gyroscope output and the accelerometer output. However, this is not the only adjustment that may be made. For example, the actual adjustment value may be a fraction of this value.
Additionally, if the segment is stationary and the angle of the rate gyroscope is drifting, a new zero angular rate measurement is made. This new zero is used for the subsequent 5 measurements of tilt by the integration of the rate gyroscope. An examplary software program for implementing the reset function is attached as schedule A. The reset function reduces accumulating integration errors.
One example of a measurable value is the joint angle between two articulating segments of a human body. In the example of sit-stand-sit motion, there are only 2 1o joints of interest: the knee joint and the hip joint. From Fig. 4, the knee joint angle, (3, is obtained, following a reset of the integrators in the computer/memory unit 24, by integrating the difference of the angular velocity signals, wZ and w3, from gyros gz and g3 respectively, according to Formula 1. The joint angle of the hip, a, is obtained in the same way using signals ~, and w2 from gyros g, and g2 respectively, and 15 substituting these variables in Formula 1 for the variables used to measure knee joint angle [3.
Upon completion of the calculations for all of the joint angles involved in a particular configuration, a second type of measurement, segment inclination, can then be made. Obtaining the inclination of the segments starts prior to movement by 2o determining the initial angle with respect to ground of the segment to which the accelerometer is attached, which in this case is the shank. Due to the DC
response of the accelerometer and its ability to measure gravity, the initial inclination, 80, of the shank is obtained by Formula 2. The symbol g represents the gravitational constant (9.81 m/s2) and Va is the sampled output of the accelerometer. The inclination of the shank thereafter, 8, is given by the addition of the integrated angular velocity output of gyro g3, w3, in Formula 3 to the initial inclination, 80. The inclination of the thigh, ~, can then be obtained by adding the angle of shank inclination, 8, to the knee joint angle, Vii. Similarly, the inclination of the pelvis, ~, is obtained by adding the angle of inclination of the thigh, fir, to the hip joint angle, a. These formulas are given as 1o Formula 4 and Formula 5. All of the segment inclinations have now been measured with respect to the ground.
A third type of measurable value, relative joint displacement or position in space, can be calculated with knowledge of joint angles and segment inclinations. A
point of origin from which to define these relative positions in space is preferably a stationary point at the end of a segment. The heel, being the end of the shank, is in contact with the ground and remains stationary for the sit-stand-sit motion making it an ideal point of origin in this example. Further, exact positions of the joints in space rely upon prior knowledge of the length of the segments. Both the ankle position and the segment lengths for the thigh and the shank need to be programmed into the 2o computer/memory unit in advance of taking measurements. Referring to Fig.
5, knowing the inclination of the shank, 8, and its length, Ls, the vertical and horizontal positions, PK~ and Pte, of the knee joint relative to the ankle joint are found with Formula 6 and Formula 7. Once this position is established, the position of the hip joint with respect to the position of the knee is determined using Formula 8 and Formula 9. Finally, the vertical and horizontal position of the hip joint, P,,~ and Putt, relative to the position of the ankle is obtained using Formula 10 and Formula 11.
Three additional intimately linked measurable kinematic values (acceleration, velocity, and position) are obtainable with the gyro-goniometer. The method of measurement, however, is better exemplified when applied to gait motion. In studying 1o human gait, researchers or clinicians are generally interested in the stride length and the average forward velocity of a patient. Though acceleration may be of less interest, it is still necessary to measure since velocity and position are directly derived from it through successive integration.
From Fig. 6, it can been seen that for normal walking, the foot is stationary in the stance phase. This stationary position provides for constant resetting of the integrator as it is a place which has a zero acceleration and zero angular velocity with constant frequency. The initial inclination of the foot, ~o, is found after reset by the static acceleration measurement of the accelerometer. However, because the acceleration is zero during this stance phase, the inclination of the foot, ~, from Fig. 7, 2o is obtained from the integral of the angular velocity signal, w,, from gyro g,.
Using the inclination of the foot, the velocity of the foot can be resolved into its horizontal and vertical components, as in Fig. 8. These values, VH and V~, are obtained with Formula 12 and Formula 13, with "a" representing the output of the accelerometer. The position of the foot, P, is obtained by taking the second integral of the accelerometer signal, or simply the integral of the velocity using Formula 14 and Formula 15. Breaking position down into its components, PX refers to the position of the foot in the direction of movement and P,, is the position of the foot vertically perpendicular to this direction. Since the integrators are reset during every stance phase, PX essentially measures the stride length during the swing phase. The forward average velocity of the subject is then taken as VH averaged over one stride length.
to An important thing to note in taking these measurements is the operation of the integrator reset function. Reset of the integrators is constantly required due to the relative nature of the measurements and the amount of error that can accumulate from successive integrations. In the case of human gait, the integrator reset function is preferably continuously carned out by comparing gyroscope and accelerometer output, but also may be an automatic response to static conditions in the movement which will reset when the accelerometer is quiet or near stationary. In this example, the computer repeats the integration process each gait cycle from an initial value, which may be for example zero.
As illustrated in the description of the method, many of the applications of this 2o invention fall into the spectrum of clinical rehabilitation and human motion study. The kinematic measurements obtainable with this invention provide feedback to clinicians so that they can establish the level of dysfunction in a patients movement and subsequently take corrective action through rehabilitation. Although the type of movement varies with location on the body, the basic nature of mechanical kinematic motion remains constant throughout. Whether the area of interest is the two dimensional motion of the leg during gait or the intricate and complex three dimensional motion of the hand and wrist, the joint angle, inclination, displacement, acceleration, and velocity are important tools in describing their motion.
However, as the physical makeup of the body changes with location, the component configuration of the this invention will change as well to accommodate for the measurement of the 1o desired kinematic values in the areas of interest.
In measuring simple joint angles, as described in the first method of measurement, in areas such as the knee, the schematic in Fig. 9 shows one possible configuration of the gyro-goniometer with gyroscopes gcl, gc2 and computer memory unit 34 with Fig. 10 displaying its physical implementation. This compact, robust, lightweight configuration only requires two gyros and offers the flexibility for use both inside and outside of a laboratory or clinic. The components are compartmentalized into two separate compact gyro sensor clusters and are attached to the articulating segments, the thigh and the shank, using elastic straps. The separate compact, lightweight computer/memory unit 34 is worn around the waist.
2o The schematic for the gyro sensor cluster is depicted in Fig. 11 and consists of a power supply 40 required to provide power to the rate gyro gcl, an analog-to-digital converter 42, and a signal transmitter 43 . The rate gyro gc 1 measures the angular velocity of the limb or segment to which it is attached. A signal proportional to this angular velocity is coupled to the analog-to-digital converter 42 where the signal is sampled and sent via a signal transmitter 43 to the computer/memory unit 34.
The 5 computer/memory unit 34 receives the signals via a signal receiver, makes the required calculations and stores the results to memory.
Based upon this configuration, a more complete analysis of lower body motion, which follow the remaining methods of measurement, may take on the component arrangement shown schematically in Fig. 14. Additional gyro sensor clusters gc3. gc4 1o are attached to the pelvis and foot while accelerometer sensor clusters acl, act are added to both the shank and the foot. The physical implementation is shown in Fig.
12. The same computer 34 may be used, programmed to handle all the sensors.
An accelerometer sensor cluster is shown schematically in Fig. 13 and is designed in the same fashion as the gyro sensor cluster. A power supply 50 supplies 15 power to the accelerometer acl The accelerometer acl measures the inclination and/or acceleration of the limb or segment to which it is attached. A signal proportional to the inclination and/or acceleration is coupled to the analog-to-digital converter 52 where the signal is sampled and sent via a signal transmitter 53 to the computer/memory unit 34. The computer/memory unit 34 receives the signals via a signal receiver, makes 2o the required calculations and stores the results to memory. As with the gyro sensor clusters, an elastic strap 55 is used attach the accelerometer sensor cluster to the shank. The accelerometer and gyro matched with the foot can be easily attached to a shoe.
This cluster configuration is very useful in measuring motion of patients outside of the clinic or laboratory where they are not under the pressure of the observing eye of the clinician or researcher. The clusters and computer/memory unit are small enough to make limb movement unencumbered and the data from the measurements can be stored over a set period of time for clinical analysis the next time the patient goes back to the clinic or laboratory. Further, by implanting the clusters into prostheses or orthoses, the clinician or researcher can indirectly observe the usage to of them in the patients normal environment.
A sizable amount of rehabilitation takes place in athletics, an area which would benefit from this configuration. Often it is difficult to truly measure and analyze the specific motions relating to a particular sport in a laboratory or clinic. The ability to put the laboratory into the sport can be attainable by incorporating the cluster configuration. In a sport such as hockey, the compact and robust design could be placed under the equipment without hampering movement. When placed in a practice or game situation, this type of measurement would give a better picture of a players injury or rate of recovery.
Additionally, the incorporation of tri-axial gyros and accelerometers into the invention would be a necessity in analyzing the movement in most sports.
Joggers and sprinters could get away with using uni-axial gyros and accelerometers since the leg motion involved in these sports is generally two-dimensional. However, in hockey, the three dimensional nature of a skating stride would require tri-axial gyros and accelerometers to accurately describe this movement.
The calculations for incorporating these multi-axial counterparts are simply an extension of the calculations previously described for uni-axial gyros and accelerometers taking into account added axes. Since the ones described are externally applied and based on orthogonality, implants would require the assistance of applications such as neural networks to account for improper alignment of the implants within the body.
1o The method of measurement described thus far has focused on the motion of the lower half of the human body. Taking advantage of the small size of micromachined gyros and accelerometers, the complex and intricate movements of the wrist and hand represent another area of human motion rehabilitation to which the invention could be applied. The complexity of this motion will demand a dii~erent component configuration for the invention. As described earlier, the configuration for the lower half of the body requires 4 gyros, 2, though preferably 4, accelerometers and has the option of clustering the components. The configuration of the hand and wrist will need as many as 20 or more gyros and 1 accelerometer. Because of the small size of the hand in comparison to the leg and the increased number of gyros, component 2o clustering would be difficult to incorporate.
As shown in Fig. 15, gyros g would be attached by elastic bands or adhesive tape to the individual finger segments to measure the angles between the two joints of every finger and the single thumb joint. Five additional gyros would be aligned with the position of the fingers and thumb and would be placed on the back of the hand to measure the angle at the knuckle joints. In order to determine the inclinations of these segments with respect to the wrist or forearm, an accelerometer may be placed on the back of the hand. However, for continuous integral error correction, it is preferred that each gyroscope be co-located with an accelerometer. By adding an additional gyro to the forearm and using it in conjunction with the gyro on the back of the hand at the knuckle of the middle finger, the angle of the wrist joint may also be determined. The 1o method of calculation for these inclinations and joint angles are simply an extension of the ones previously described taking into account the added number of joints and gyros.
This hand/wrist configuration would utilize an external computer/memory unit, analog-to-digital converter, cables to couple the components in series, uni-axial gyros and a uni-axial accelerometer. Additional measurements to take into account the three dimensional movement of the hand would require extra uni-axial gyros on the back of the hand. However, since this invention is not meant to limit the usage of gyros and accelerometers to those of uni-axial orientation, the substitution of tri=axial gyros for uni-axial on the back of the hand would be able to take into account its curved motion.
2o A tri-axial gyro substituted on the forearm would make it possible to measure the side to side motion of the hand at the wrist joint.
Another area of use for the gyro-goniometer in rehabilitation would be in fi~nctional electrical stimulation (FES) treatment. FES systems are design to electrically provide stimulation to the muscles of people suffering from conditions such as cerebral palsy or spinal cord injury. In enabling paralyzed people to walk or move, these stimulations help to produce useful muscle contraction for limb movement.
Where the gyro-goniometer would play a role is in the feedback control position.
For example, in a sit-stand-sit maneuver, the gyro-goniometer may be configured as shown in Fig. 4 by neglecting segment 4 and by attaching a gyro sensor cluster to the pelvis, the thigh, and the shank, representing segments 1, 2, and 3.
l0 Inclinations and relative positions with respect to the ground are found by attaching an accelerometer to the shank as well. This configuration could be used to help navigate movement using the ankle joint as the origin for relative inclinations. The position of the hip joint could be used by the FES controller to estimate the position of the seat surface and thereby allow the stimulation to be adjusted as to reduce the impact velocity of the buttocks at the termination of the FES controlled sit-down procedure.
The methods of measurement follow those previously described with the calculated values being stored in memory for the control unit to extract and use for controlling stimulation.
The gyro-goniometer may be extended to similar applications of control 2o feedback outside of human motion rehabilitation in areas such as machine learning. A
machine learning how to walk by itself without anyone having to tell it how needs reinforcement signals to punish the controller when it performs a wrong movement.
Some examples of wrong movement: if the toe contacts the ground before the heel, if the knee is not extended at the end of the swing phase just prior to loading it, or if there has been no stride forward in which the hip angle is not forward.

5 By integrating the invention into the control feedback loop of a machine, the controller would be able to predict that the toe is going to hit the floor, that the knee is not going to be fully extended at the end of the swing, or that the hip angle is not advancing forward. This knowledge could be used to self adapt and take avoidance action. The number of gyros and accelerometers would, again, depend upon the level to of control desired and the complexity of the motion. Further applications of this invention may rely on information mapping based upon the movement of a human, animal, robot or other moving objects. In virtual reality applications, for example, the movement of a human may be mapped into cyber space.
Software for carrying out the method of the invention is as follows. MASS is 15 the main program. It calibratres the sensors via colo cal, grey cal and pgcal. MASS
analyzes three different types of examples. One is the calculation of tilt of a segment (proc is with analyses handled by an fl a). Another is calculation of angle of a segment with reference to a non-moving segment (proc ss with analysis handled by ss anl-a). The final example is calculation of the angle between two moving segments 2o (proc ms with analysis handled by bgy_anl-a). F6 READ describes each of the data files that are used by the program.
A person skilled in the art could make immaterial modifications to the invention described in this patent document without departing from the essence of the invention that is intended to be covered by the scope of the claims that follow.

Schedule A
$MASS.m %This program handles the full analysis of the data. The calibration files are handled by the sub routines of colo cal and grey cal, as well as pgcal.
% All code that preceded in a line by the'%' character is a comment clear [p0,p90]=pgcal( 1 ); %Calibrate the penny and giles [c a max,c a min,c b max,c b min]=colo cal(1); %Calibrate the coloured cluster [g_a max,g a min,g b max,g b min]=grey_cal(1); %Calibrate the grey sides %tl is used for correlation of the accelerometers to the Penny and Giles goniometer (Fig. 3)..
cr ac-pg %Start the processing of the data files.
2o proc is %This subroutine handles the calculation for a single sensor cluster proc ss %This subroutine handles the calculation for a single cluster with reference to an unmoving cluster proc_ms %This subroutine handles the calculation for the generation of angles between two clusters.
%pgcal fiznction [p0,p90]=pgcal(junk);
%This program does the Penny and Giles calibration routine. It returns the p0 and p90 values 3o fid=fopen('p0.bin');
in=fread(fid,1,'int32');
x=fread(fid, [in inf],'float64');
cl=x;
fclose(fid);
fid=fopen('p90.bin');
in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
c2=x ;
4o fclose(fid);
p0=mean(c 1 );
p90=mean(c2);
fixnction [c a max,c a_min,c_b_max,c_b_min]=colo_cal(junk);
%This subroutine takes the calibration and calculates the neccessary values of %Minimum and maximums for the calculation of the angles %The files of 'red?.bin' are the files that were used in the calibration yechnique fid=fopen('red0.bin');
in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
to c0=x';
fclose(fid);
fid=fopen('redl .bin');
in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
cl=x';
fclose(fid);
fid=fopen('red2.bin');
2o in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
c2=x';
fclose(fid);
fid=fopen('red3.bin');
in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
c3=x ;
fclose(fid);
%Find the 2nd and 3rd only sensor signals, these are the ones for calibration a=1;
b=400;
c calf=[c0(a:b,2) cl(a:b,2) c2(a:b,2) c3(a:b,2) c0(a:b,3) cl(a:b,3) c2(a:b,3) c3(a:b,3)];
cmean=mean(c cali);
%Calulates the limits of the calibration c a_max=max(cmean( 1:4));
4o c a_min=min(cmean( 1:4));
c b max=max(cmean(5:8));
c b min=min(cmean(5:8));
range ca=c_a max-c a min;
range cb=c b max-c b min;

%This checks the accuracy of the calibration c angs=[2*(cmean(1)-c_a_min)/range ca-1 2*(cmean(5)-c_b_min)/range cb-1;
2*(cmean(2)-c a_min)/range ca-1 2*(cmean(6)-c b_min)/range cb-1; 2*(cmean(3)-c a_min)/range ca-1 2*(cmean(7)-c_b min)/range cb-1; 2*(cmean(4)-to c a min)/range ca-1 2*(cmean(8)-c b min)/range_cb-1];
c angs=atan2(c angs(:,1),c angs(:,2))* 180/pi;
fiznction [g a max,g a min,g b max,g b min]=grey cal(junk);
%This subroutine takes the calibration and calculates the neccessary values of %Minimum and maximums for the calculation of the angles %This follows the same structure as colo cal 2o fid=fopen('gre0.bin');
in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
gr0=x ;
fclo se(fid);
fid=fopen('gre l .bin');
in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
grl=x';
3o fclose(fid);
fid=fopen('gre2. bin');
in=fread(fid, l,'int32');
x=fread(fid,[in infJ,'float64');
gr2=x';
fclose(fid);
fid=fopen('gre3.bin');
in=fread(fid,1,'int32');
4o x=fread(fid,[in infJ,'floatb4');
gr3 ~c ;
fclose(fid);
%Take the first two only a=l;
b=400;
g cali=[gr0(a:b,l) grl(a:b,l) gr2(a:b,l) gr3(a:b,l) gr0(a:b,2) grl(a:b,2) gr2(a:b,2) gr3(a:b,2)];
gmean=mean(g cali);
g_a max=max(gmean( 1:4));
g_a min=min(gmean( 1:4));
to g b max=max(gmean(5:8));
g b min=min(gmean(5:8));
rangeJga=g a max-g a mln;
rangeJgb=g b max-g b min;
g angs=[2*(gmean(1)-g_a min)/rangeJga-1 2*(gmean(5)-g b min)/rangeJgb-l;
2*(gmean(2)-g a min)/rangeJga-1 2*(gmean(6)-g b min)/rangeJgb-1; 2*(gmean(3)-g a min)/range~ga-1 2*(gmean(7)-g b min)/range~,gb-1; 2*(gmean(4)-g_a min)/rangerga-1 2*{gmean(8)-g b min)/range-gb-1];
2o g ang=atan2(g angs(:,1),g angs(:,2))* 180/pi %Proc ts.m %This file handles the opening and closing of the multiple data files processed %for the calculation of tilt by the gyrogoniometer %The files 't?.bin' are data files that have been previously recorded.
%an fl a is the files used to process the data files index=1;
fid=fopen('t2.bin');
3o in=fread(fid, l,'int32');
x=fread(fid,[in inf],'float64');
t2=x ;
fclose(fid);
clear x an_fl_a %break fid=fopen('t3.bin');
4o in=fread(fid,1,'int32');
x=fread(fid, [in inf~,'float64');
t2=x ;
fclose(fid);
clear x an fl a fid=fopen('t4.bin');
in=fread(fid, l,'int32');
x=fread(fid,[in infJ,'float64');
t2=x ;
1o fclose(fid);
clear x an fl a 15 fid=fopen('t5.bin');
in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
fclose(fid);
20 clear x an fl a fid=fopen('t6.bin');
25 in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
fclose(fid);
clear x an_fl_a RMS
%anfla %this file processes the data for the calculation of tilt from two clusters %One cluster is referred to as gray (gr), one as colored (co) %First, low pass filter the signals szt2=size(t2,1 );
[b,a]=butter(2,0.1);
for i=1:8;x(:,i)=filter(b,a,t2(:,i));end %OK. There is some delay added to the penny and giles goniometer due to the recording procedures, so the signal is not delayed using the analog filter x(:,9)=filtfilt(b,a,t2(:,9));
%Now calculate the angles from the two accelerometer readings ang_pg=(x( :, 9)-p0)/(p90-p0) * 90;
ang-gr=atan2(2*(t2(:,4)-g a min)/(g a max-g a min)-1,2*(t2(:,5)-g b min)/(g b max-g b min)-1)*180/pi;
ang co=atan2(2*(t2(:,2)-c a_min)/(c a max-c a_min)-1,2*(t2(:,3)-l0 c_b min)/(c b max-c b_min)-1)*180/pi;
for i=l:size(ang-gr,l);if(angJgr(i)<0) ang_gr(i)=ang-gr(i)+360;end;if(ang co(i)<0) ang co(i)=ang co(i)+360;end;end angJgl=ang-gr-g offset;
ang_co=(ang_co-c offset)*(-1);
%Ok. These are looking quite solid at this point. Next is the adjustment of the gyroscopes. This is done by looking at the actual accelerometer signals and not the ones that are filtered and transformed.
%First take an integral of the gyroscopic signal to see how bad it could be without adjustment. Take three consecutive bins of 15 sample, and evaluate every 10 samples.
Take the initial gyroscope offset from 50:150 (assuming stationary) %this calculates the zero reading for each of the two gyroscopes gy r o~mean(x(51:150,1));
gyp o~mean(x(51:150,6));
%this sets the size of the matrix for the integral of eachg rate gyroscope coJgo un=zeros(szt2,1);

gr~go un=zeros(szt2,1);

coJgo re=zeros(szt2,1);

grJgo re=zeros(szt2,1);

%This sets the initial tilt reading to the initial reading of the goniometer coJgo un(51:150)=ang_pg(51:150);

gr_,go un(51:150)=ang_pg(51:150);

co_go re(51:150)=ang-pg(51:150);

gr-go re(51:150)=ang~g(51:150);

%This calculates the integral of the rate gyroscopes for i=151:szt2 co-go un(i)=((t2(i,l)+t2(i-1,1))/2 - gy_r of)/(-100)/0.0011 + co-go un(i-1);
gr_,go un(i)=((t2(i,6)+t2(i-1,6))/2 - gyJg of)/(100)/0.00092 + gr-,go un(i-1);
end to %This routine checks for adjustment of the integral for i=151:szt2;
%This line means that the adjustment is checked for every 20 samples %This is handles in a causal fashion that is necessary for control applications if(floor(i/20)==1/20) Possibly this line could be omitted.
%This takes one of the accelerometer signals past history of 45 samples and checks to see if it is not moving (as determined by the test sum c 15 threshold) bin_am=mean(x(i-44:1-30,2:3));
bin_bm=mean{x(i-29:1-15,2:3));
bin cm=mean(x(i-14:1,2:3));
2o test sum_c=((bin_am-bin_bm).~2+(bin_am-bin_cm).~2+(bin bm-bin_cm).~2)*[1;1];
if(test sum_c < .2e-5) %Reset the gyroscope and its angle to the acct angle %i %gy_r of %co-go re(i-1) 25 %mean(ang co(i-44:1)) %gy_r o~mean(x(i-44:1,1 )) co-go re(i-1)=mean(ang co(i-44:1));
%This is the above routine calculated for the second cluster %additionally, if the difference between the rate gyroscope and the accelerometer is 3o too high, the integral of the rate gyroscope is adjusted to the accelerometer calculation if(abs(mean(co-go re(i-45:1-1))-mean(ang co(i-45:1-1))) > 8) co_go re(i-1)=mean(ang co(i-44:1));
end end bin am=mean(x(i-44:1-30,4:5));
bin_bm=mean(x(i-29:1-15,4:5));
bin cm=mean(x(i-14:1,4:5));
test sum_g=((bin_am-bin_bm).~2+{bin_am-bin_cm).~2+(bin_bm-bin cm).~2)*[1;1];
if(test sumJg < 2e-4) %Reset the gyroscope and its angle to the accl angle i l0 ~s [gyp of mean(x(i-44:1,6))]
gyp o~mean(x(i-44:1,6));
[ang_pg(i-1) gr~go re(i-1) mean(ang_gr(i-44:i))]
gr~go re(i-1)=mean(ang_,gr(i-44:i));
end %The other resetting will come in if the gyroscope strays more than 5 degrees from %the acclerometer signal if(abs(mean(coJgo re(i-45:i-1))-mean(ang co(i-45:i-1))) > 8) coJgo re(i-1)=mean(ang co(i-44:i));
end end %This calculates the integral of the rate gyroscope signal co-go re(i)=((t2(i,l)+t2(i-1,1))/2 - gy-r of)/(-100)/0.0011 + coJgo re(i-1);
gr_,go re(i)=((t2(i,6)+t2(i-1,6))/2 - gy-,g-of)/(100)/0.00092 + gr-go re(i-1);
end figure %To see the actual signals, plot out the results subplot(211 );xa=[ 101: szt2]/100;p1ot(xa,(ang_pg( 101: szt2)-co-go un( 101: szt2)),xa, (ang_pg( 101: szt2)-co~go re( 101: szt2))) 3o title('Coloured Cluster Blue - Un, Green - re');grid %figure plot(xa,ang-pg(lOl:szt2),xa,coJgo un(lOl azt2),xa,co-go re(lOl:szt2),xa,grJgo a n( 101: szt2),xa,gr_,go re( 1 O 1: szt2)) %grid %title('Comparison of the angles for all the methods') %figure subplot(212);plot(xa, (ang_pg( 1 O 1: szt2)-grJgo un( 1 O 1: szt2)),xa, (ang_pg( 101: szt2)-gr~go re( 1 O 1: szt2))) title('Grey Cluster Blue - Un, Green - re');grid %A simple RMS error analysis to check the results:
RMS (index, l )=sqrt((ang-pg( 101: szt2)-co-go un{ 101: szt2))' * (ang_pg( 101: szt2)-co_go un(lOl:szt2))/(szt2-100));
RMS(index,2)=sqrt((ang-pg(lOl:szt2)-co_,go re(lOl:szt2))'*(ang-pg(lOl:szt2)-co~o re( 1 O 1: szt2))/(szt2-100));
RMS(index,3)=sqrt((ang_pg(lOl:szt2)-gr_,go un(lOl:szt2))'*(ang-pg(lOl:szt2)-gr_go un(101 azt2))/(szt2-100));

RMS(index,4)=sqrt((ang-pg( 1 O 1: szt2)-gr_go re( 1 O 1: szt2))' * (ang_pg( 1 O 1: szt2)-gr~go re(lOl:szt2))/(szt2-100));
RMS(index, 5)=sqrt((ang_pg( 1 O l : szt2)-ang co( 101: szt2))' * (ang-pg( 1 O
1: szt2)-ang_co( 101: szt2))/(szt2-100));
RMS(index,6)=sqrt((ang~g( 101: szt2)-ang-gr( 101: szt2))'*(ang-pg( 101: szt2)-to ang-,gr(lOl:szt2))/(szt2-100));
RMS
index=index+1;
%This files processes the data files taken for the calculation of angle from one %cluster that is moving and one that is stationary %the files 's?.bin' are the data files that are processed %the file ss anl_a is the file that is used to process 2o fid=fopen('sl.bin');
in=fread(fid,1,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
fclose(fid);
clear x ss and a fid=fopen('s2.bin');
in=fread(fid, l,'int32');
3o x=fread(fid,[in infJ,'float64'};
t2=x ;
fclose(fid);
clear x ss and a fid=fopen('s3 . bin');
in=fread(fid, l,'int32');
x=fread(fid,[in inf],'float64');
t2=x';
4o fclose(fid);
clear x ss and a

10 %ss and a %First, filter the signals %Again, two clusters are used, one is referred to as colored, one as grey szt2=size(t2,1 );
[b,a]=butter(2,0.1 );
for i=1:8;x(:,i)=filter(b,a,t2(:,i));end %Due to delay added to the penny and giles goniometer by the recording procedures, 15 the signal is not delayed.
x(:,9)=filtfilt(b,a,t2(:,9));
%Calculate the angles from the two accelerometer readings ang_pg=(x(:, 9)-p0)/(p90-p0) * 90;
2o ang-gr=atan2(2*(t2(:,4)-g a min)/(g a max-g a min)-1,2*(t2(:,5)-g b_min)/(g b max-g b min)-1)*180/pi;
ang co=atan2(2*(t2(:,2)-c a_min)/(c a max-c a_min)-1,2*(t2(:,3)-c_b_min)/(c b max-c_b_min)-1)*180/pi;
%The coloured accelerometer/gyro cluster is staying stationary, 25 %The offset reference so that this matches with the PG is ang dif~20.5283 %This calculates the angle by the accelerometer method 3o for i=l:size(angJgr,1);if(ang-gr(i)<0) angJgr(i)=angJgr(i)+360;end;if(ang co(i)<0) ang co(i)=ang co(i)+360;end;end ang_gl=ang-gr-g offset;
ang co=(ang co-c offset)*(-1);
35 ang al-pg=angJgr-ang co+ang diff;
%figure %plot(ang-pg) %hold on;plot(ang al-pg,'r');
%Now reset the gyroscopes. Look at the actual accelerometer signals and not the ones that are filtered and transformed. Take an integral of the gyroscopic signal to see how bad it could be. Take three consecutive bins of 15 sample, and evaluate every samples. Take the initial gyroscope offset from 50:150 (assuming stationary) %This is the initial calculation of the zero of the rate gyroscope gy_r o~mean(x(51:150,1 ));

gy-g o~mean(x(51:150,6));
%This sets the size of the matrix for the unadjusted rate gyroscope integral co_,go un=zeros(szt2,1);
gr-go un=zeros(szt2,1);
1o coJgo re=zeros(szt2,l);
grJgo re=zeros(szt2,1 );
%This sets the initial value of the rate gyroscope integral co-go un(51:150)=ang_pg(51:150);
15 gr-go un(51:150)=ang~g(51:150);
coo re(51:150)=ang~g(51:150);
grJgo re(51:150)=ang~g(51:150);
%This calculates an unadjusted integral for the rate gyrsoscope 2o for i=151:szt2 coJgo un(i)=((t2(i,l)+t2(i-1,1))/2 - gy r of)/(-100)/0.0011 + co-go un(i-1);
gr~go_un(i)=((t2(i,6)+t2(i-1,6))/2 - gyp ofJ/(100)/0.00092 + gr_,go un(i-1);
end %This is the method for calculating the adjusted integral of the rate gyroscope %see an_fl a for line by line description for i=151:szt2;
3o if(floor(i/20)=i/20) bin_am=mean(x(i-44:i-30,2:3));
bin_bm=mean(x(i-29: i-15, 2: 3 ));
bin_cm=mean(x(i-14: i, 2: 3 ));
test_sum_c=((bin am-bin_bm).~2+(bin am-bin_cm).~2+(bin bm-bin cm).~2)*[1;1];
if(test sum_c < 2.Se-5) %Reset the gyroscope and its angle to the accl angle %i %gy r of %co-,go re(i-1) %mean(ang_co(i-44: i)) %gy-r o~mean(x(i-44: i, l )) co-go re(i-1)=mean(ang co(i-44:i));
end %The other resetting will come in if the gyroscope strays more than 5 degrees from the acclerometer signal if(abs(mean(co-go re(i-45:i-1))-mean(ang co(i-45:i-1))) > 8) coJgo re(i-1)=mean(ang co(i-44:i));
end bin am=mean(x(i-44:i-30,4:5));
bin_bm=mean(x(i-29:i-15,4:5));
bin cm=mean(x(i-14:1,4:5));
test sumJg=((bin am-bin_bm).~2+(bin am-bin cm).~2+(bin bm-bin cm).~2)*[1;1];
angle if(test sumJg < 3e-4) %Reset the gyroscope and its angle to the accl [gyp of mean(x(i-44:1,6))]
gyp o~mean(x(i-44:1,6));
[ang-pg(i-1) gr~go re(i-1) mean(ang_gr(i-44:1))]
grJgo re(i-1)=mean(ang-gr(i-44:1));
end %The other resetting will come in if the gyroscope strays more than 5 degrees from the acclerometer signal if(abs(mean(coJgo re(i-45:1-1))-mean(ang co(i-45:1-1))) > 8) 3o coJgo re(i-1)=mean(ang_co(i-44:1));
end end co-go re(i)=((t2(i,1)+t2(i-1,1))/2 - gy-r of)/(-100)/0.0011 + coJgo re(i-1);
grJgo re(i)=((t2(i,6)+t2(i-1,6))/2 - gy_,g of)/(100)/0.00092 + grJgo re(i-1);
end %Take the difference of the two angles %figure %plot(ang-pg);
%hold on;
ang_grl~g=gr_go re-co-go re+ang diff;
ang-gulag=grJgo un-coJgo un+ang diff+63;
%plot(ang~grl~g,'r');
%plot(ang-gulag+63,'g');grid figure %Plots the results xa=[201: szt2];plot(xa, (ang_pg(201: szt2)-angJgrl-pg(201: szt2)),xa, (ang-pg(201: szt2)-ang-gul-pg(201: szt2))) 1o title('Reset (y) and Unreset (g) gyro errors');grid %figure %plot(xa, (ang-pg( 101: szt2)-gr_go un( 101: szt2)),xa, (ang-pg( 101: szt2)-gr-,go re( 101: szt2))) %title('Grey Cluster - Yellow - Un, Purple - re, Cyan - accel');grid %proc ms %This handles the data files in which both clusters are moving. The two clusters are referred to as gray and colored. The files are'm?.bin'. ms anl_a is used to calculate the 2o angle from each file.
corn-un=0;
corr_re=0;
fid=fopen('ml .bin');
in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
t2=x';
fclose(fid);
clear x ms_anl_a RMS_re=sqrt((ang-pg(201: szt2)-ang-grl-pg(201: szt2))'*(ang-pg(201: szt2)-ang~grl-pg(201: szt2))/(szt2-200)) RMS_un=sqrt((ang-pg(201: szt2)-ang Jgul-pg(201: szt2)-23 )' * (ang_pg(201:
szt2)-ang-gul~g(201: szt2)-23 )/(szt2-200)) break corn un=-15;
corn-re=0;
fid=fopen('m2.bin');
4o in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
fclose(fid);
clear x ms and a RMS_re=sqrt((ang-pg(201: szt2)-angJgrl-pg(201: szt2))'*(ang-pg(201: szt2)-ang-grl~g(201: szt2))/(szt2-200)) RMS_un=sqrt((ang-pg(201: szt2)-angJgul-pg(201: szt2)-23 )'* (ang-pg(201: szt2)-angJgul-pg(201 azt2)-23)/(szt2-200}) to con_un=-14;
fid=fopen('m3.bin');
in=fread(fid, l,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
is fclose(fid);
clear x ms_anl_a RMS_re=sqrt((ang~g(201: szt2)-ang-grl-pg(201: szt2))' * (ang-pg(201: szt2)-2o angJgrl-pg(201:szt2))/(szt2-200)) RMS_un=sqrt((ang~g(201: szt2)-ang_gul-pg(201: szt2)-23 )' * (ang_pg(201: szt2)-ang-gul-pg(201: szt2)-23 )/(szt2-200)) %corr un=-24;
25 %corr-re=-6;
fid=fopen('m4.bin');
in=fread(fld,1,'int32');
x=fread(fid, [in infJ,'float64');
t2=x ;
3o fclose(fid);
clear x %ms and a %RMS re=sqrt((ang_pg(201:szt2)-angJgrl_pg(201:szt2))'*(ang-pg(201:szt2)-35 angJgrl_pg(201:szt2)}/(szt2-200)) %RMS un=sqrt((ang-pg(201:szt2)-ang_gul-pg(201:szt2)-23)'*(ang-pg(201:szt2)-ang-gul-pg(201: szt2)-23)/(szt2-200)) 40 fid=fopen('m5.bin');
in=fread(fid, l,'int32');
x=fread(fid,[in infJ,'float64');
t2=x ;
fclose(fid);
45 clear x ms_anl_a RMS_re=sqrt((ang~g(201: szt2)-ang Jgrl-pg(201: szt2))' * (ang~g(201: szt2)-ang_grl-pg(201: szt2))/(szt2-200)) RMS_un=sqrt((ang~g(201: szt2)-ang_gul-pg(201: szt2)-23)'*(ang-pg(201: szt2)-ang_gul-pg(201: szt2)-23 }/(szt2-200)) to %pause con un=-20;
fid=fopen('m6.bin');
15 in=fread(fid, l,'int32');
x=fread(fid,[in infJ,'float64');
t2=x ;
fclose(fid);
clear x ms_anl_a RMS_re=sqrt((ang-pg(201: szt2)-ang-grl-pg(201: szt2))' * (ang~g(201: szt2)-angJgrl~g(201 azt2))/(szt2-200)) RMS_un=sqrt((ang-pg(201: szt2)-angJgul-pg(201: szt2)-23 )'* (ang_pg(201: szt2)-angJgul~g(201 azt2)-23)/(szt2-200)) %break fid=fopen('m7.bin');
in=fread(fid, l,'int32');
3o x=fread(fid,[in infJ,'float64');
t2~';
fclose(fid);
clear x %ms and a %RMS re=sqrt((ang_pg(201:szt2)-ang-grl-pg(201:szt2))'*(ang_pg(201:szt2)-ang-grl~g(201: szt2))/(szt2-200)) %RMS un=sqrt((ang~g(201:szt2)-angJgul~g(201:szt2)-23)'*(ang~g(201:szt2)-ang_gul-pg(201: szt2)-23 )/(szt2-200)) %ms and a %First, filter the signals. The processing technique has been described in the previous files, though it is slightly different here due to the different application.
%ss and a and an fl a szt2=size(t2,1 );
[b, a]=butter(2, 0.1 );
for i=1:8;x(:,i)=filter(b,a,t2(:,i));end x(:,9)=filtfilt(b,a,t2(:,9));
%Calculate the angles from the two accelerometer readings ang-pg=(x(:, 9)-p0)/(p90-p0) * 90;
ang~gi=atan2(2*(t2(:,4)-g a min)/(g_a max-g a min)-1,2*(t2(:,5)-g b min)/(g b max-g b min)-1 ) * 180/pi;
ang co=atan2(2*(t2(:,2)-c a min)/(c a max-c a_min)-1,2*(t2(:,3)-c_b_min)/(c b max-c_b_min)-1)*180/pi;
%The coloured accelerometer/gyro cluster is staying stationary. The offset reference 2o so that this matches with the PG is ang diff=20.5283;
for i=l:size(ang~gr,l);if(angJgr(i)<0) ang~gr(i)=ang-gr{i)+360;end;if(ang co(i)<0) ang co(i)=ang co(i)+360;end;end ang-gr=angJgr-g offset;
ang co=(ang co-c offset)*(-1);
ang al_pg=ang-,gr-ang co+ang diff;
%Reset gyroscopes. Look at the actual acclerometer readings. Take an integral of the 3o gyroscopic signal to see how bad it could be. Take three consecutive bins of 15 sample, and evaluate every 10 samples Take the initial gyroscope offset from 50:150 (assuming stationary).
gy r o~mean(x(51:150,1));
gyp o~mean(x(51:150,6));
co-go un=zeros(szt2,1);
grJgo un=zeros(szt2,1);
coJgo re=zeros(szt2,1);
grJgo re=zeros(szt2,1);
co-go un(51:150)=ang~g(51:150);

gr-go un(51:150)=ang-pg(51:150);

coJgo re(S ang_pg(51:150);
1:150)=

grJgo re(51:150)=ang_pg(51:150);

for i=151:szt2 coJgo un(i)=((t2(i,l)+t2(i-1,1))/2 - gy-r of)/(-100)/0.0011 + co~go un(i-1);
gr_,go un(i)=((t2(i,6)+t2(i-1,6))/2 - gyp of)/(100)/0.00092 + gr-,go un(i-1);
end for i=151:szt2;
l0 %if(floor(i/20)=i/20) bin_am=mean(x(i-44:i-30,2:3));
bin_bm=mean(x(i-29:i-15,2:3));
bin cm=mean(x(i-14:1,2:3));
15 test sum_c=((bin am-bin_bm).~2+(bin am-bin_cm).~2+(bin bm-bin cm}.~2)*[1;1];
if(test sum_c < l0e-5) Reset the gyroscope and its angle to the accl angle %i 20 %gy r of %co Jgo re(i-1 ) %mean(ang co(i-44:1)) %gy r o~mean(x(i-44: i, l )) coJgo re(i-1)=mean(ang co(i-44:1));
25 end %The other resetting will come in if the gyroscope strays more than 5 degrees from the acclerometer signal if(abs(mean(coJgo re(i-45:1-1))-mean(ang co(i-45:1-1))) > 1) 3o coo re(i-1)=mean(ang co(i-19:1));
end bin_am=mean(x(i-44:1-30,4:5));
bin_bm=mean(x(i-29: i-15,4: 5 ));
35 bin cm=mean(x(i-14:1,4:5));
test sumJg=((bin am-bin_bm).~2+(bin am-bin cm).~2+(bin bm-bin cm).~2)*[1;1];
4o if{test sumJg < 6e-4) %Reset the gyroscope and its angle to the accl angle i [gyp of mean(x(i-44:1,6))]
gyp o~mean(x(i-44:1,6));
[ang_pg(i-1 ) gr-go re(i-1 ) mean(ang-gr(i-44: i))]
45 gr_go re(i-1)=mean(angJgr(i-44:1));
end l0 %The other resetting will come in if the gyroscope strays more than 5 degrees from the acclerometer signal if(abs(mean(gr-go re(i-45:i-1))-mean(angJgr(i-45:i-1))) > 2) gr_go re(i-1)=mean(angJgr(i-19:i));
end %end co-go re(i)=((t2(i,1 )+t2(i-1,1 ))/2 - gy-r of)/(-100)/0.0011 + coo re(i-1 );
15 gr-go re(i)=((t2(i,6)+t2(i-1,6))/2 - gyp of)/(100)/0.00092 + grJgo re(i-1);
end %Take the diiTerence of the two angles xa=[201: szt2]/ 100;
2o figure plot(xa,ang_pg(201:szt2),xa,ang al_pg(201:szt2)) title('Angle from accels only') RMS= sqrt(((ang al_pg(201: szt2)-ang-pg(201: szt2))'*(ang al-pg(201: szt2)-ang~g(201: szt2)))/(szt2-200)) figure ang-grl-pg=grJgo re-coo re+ang diff+corr_re;
ang_gul_pg=grJgo un-coJgo un+ang diff+63+corr_un;
plot(xa, ang_pg(201: szt2), xa, ang-grl-pg(201: szt2),xa, ang-gul_pg(201:
szt2)+23 );
grid legend('Gonio','Reset','Unreset');
figure %Find the error signals plot(xa, (ang_pg(201: szt2)-ang_grl-pg(201: szt2)),xa, (ang~g(201: szt2)-ang-gul~g(201:szt2)-23)) title('Reset (y) and Unreset (g) gyro errors');grid %figure %plot(xa, (ang_pg( 101: szt2)-gr_,go un( 101: szt2)),xa, (ang~g( 101: szt2)-gr-go re( 1 O 1: szt2))) %title('Grey Cluster - Yellow - Un, Purple - re, Cyan - accel');grid %break Various thresholds may be used instead of the 5 degree threshold. A smaller value is preferred for more accurate adjustment. A preferred threshold is about 1-2 degrees.

Claims (52)

THE EMBODIMENTS OF THE INVENTION IN WHICH AN EXCLUSIVE
PROPERTY OR PRIVILEGE IS CLAIMED ARE DEFINED AS FOLLOWS:

1. A method of detecting angular motion, the method comprising the steps of:
sensing a first angular rate of a first moving body and producing a first angular rate output;
integrating the first angular rate output to produce a first angle signal;
and adjusting the first angle signal to reduce accumulating integration errors in the first angle signal.

2. The method of claim 1 in which the first angle signal is adjusted manually.

3. The method of claim 1 in which adjusting the first angle signal comprises comparing the first angle signal to a reference signal and adjusting the first angle signal when the difference between the first angle signal and the reference signal exceeds a given level.

4. The method of claim 3 in which the reference signal is a gravitational reference signal.

5. The method of claim 4 in which the reference signal is obtained from output from an accelerometer.

6. The method of claim 5 in which the output from the accelerometer is filtered to produce the reference signal.

7. The method of claim 6 in which the first angle signal is adjusted to a value determined from the reference signal.

8. The method of claim 7 in which the first angle signal is adjusted to reduce the difference between the first angle signal and the reference signal.

9. The method of claim 8 in which the first angle signal is adjusted by setting the first angle signal to the value of the reference signal adjusted for any delay in the reference signal caused by filtering.

10. The method of claim 1 further comprising the steps of:
sensing a second angular rate of a second moving body and producing a second angular rate output; and integrating the difference between the first and second angular rate outputs to produce the first angle signal.

11. The method of claim 10 in which the first moving body is the shank of an animal and the second moving body is the thigh of an animal.

12. The method of claim 11 further comprising the step of operating on stored position data of the first and second bodies and the first angle signal to update the stored position data.

13. The method of claim 1 further comprising the steps of:
sensing an initial inclination of a stationary body and producing an initial inclination output; and summing the first angle signal and the initial inclination output to produce a second angle signal.

14. The method of claim 13 further comprising the steps of sensing a second angular rate of a second moving body and producing a second angular rate output; and integrating the difference between the first and second angular rate outputs to produce the first angle signal.

15. The method of claim 14 in which the first moving body is the shank of an animal and the second moving body is the thigh of an animal.

16. The method of claim 14 in which the first moving body is the shank of an animal and the stationary body is the foot of an animal.

17. A goniometer, comprising:
a first angular rate sensor having, in operation, a first angular rate output;
a computer connected via a first communication link to the first angular rate sensor to receive the first angular rate output; and the computer being configured to act as an integrator to integrate the first angular rate output to produce a first angle signal and adjust the first angle signal.

18. The goniometer of claim 17 further comprising a manual reset button connected via a reset communications link to the computer, the computer being configured to adjust the first angle signal upon receipt of a signal from the manual reset button.

19. The goniometer of claim 17 further comprising:

a source of a reference signal connected to provide the reference signal to the computer; and the computer being configured to compare the first angle signal to the reference signal and adjust the first angle signal when the difference between the first angle signal and the reference signal exceeds a given level.

20. The goniometer of claim 19 in which the reference signal is a gravitational reference signal.

21. The goniometer of claim 20 in which the source of the reference signal is an accelerometer.

22. The goniometer of claim 21 further comprising a filter for filtering the output from the accelerometer to produce the reference signal.

23. The goniometer of claim 22 in which the computer is configured to adjust the first angle signal to a value determined from the reference signal.

24. The goniometer of claim 23 in which the computer is configured to adjust the first angle signal to reduce the difference between the first angle signal and the reference signal.

25. The goniometer of claim 8 in which the computer is configured to adjust the first angle signal by setting the first angle signal to the value of the reference signal adjusted for any delay in the reference signal caused by filtering.

26. The goniometer of claim 17 further comprising:
a second angular rate sensor having, in operation, a second angular rate output, the second angular rate sensor being connected to provide the second angular rate output via a second communications link to the computer, the first and second angular rate outputs having a difference between them, the computer being configured to act as an integrator to integrate the difference between the first and second angular rate outputs and generate the first angle signal.

27. The goniometer of claim 26 in which the first and second angular rate sensors are connected to moving parts of an animal.

28. The goniometer of claim 27 further comprising the computer having a memory storing position data of the moving parts of the animal.

29. The goniometer of claim 17 further comprising:
an accelerometer having, in operation, an accelerometer output, the accelerometer being connected via a third communications link to the computer;
and the computer being configured to sum processed output from the accelerometer with the first angle signal.

30. The goniometer of claim 29 in which the accelerometer and first angular rate sensor are connected to moving parts of an animal.

31. A goniometer, comprising:
a first angular rate sensor having, in operation, a first angular rate output;
an accelerometer having, in operation, an accelerometer output; and a computer connected via first and second communications links respectively to receive the accelerometer output and the first angular rate output, the computer being configured to act as an integrator to integrate the first angular rate output, generate a first angle signal and add the first angle signal to the accelerometer output.

32. The goniometer of claim 31 further comprising:
a second angular rate sensor having, in operation, a second output, the second angular rate sensor being connected via a third communications link to the computer, the first and second outputs having a difference between them; and the computer being configured to act as an integrator to integrate the difference between the first and second angular rate outputs and generate a second angle signal.

33. The goniometer of claim 32 in which the computer further comprises a memory containing stored position data, the computer being configured to operate on the stored position data and one or both of the first and second angle signals to generate at least a third angle signal.

34. The goniometer of claim 31 in which the computer is configured to repeatedly integrate the first annular rate output from an initial value.

35. A goniometer, comprising:
a first angular rate sensor having, in operation, a first angular rate output;
a second angular rate sensor having, in operation, a second angular rate output; and a computer connected via first and second communications links respectively to receive the first and second angular rate outputs, the first and second angular rate outputs having a difference between them, the computer being configured to act as an integrator to integrate the difference between the first and second angular rate outputs and generate an angle signal.

36. The goniometer of claim 35 in which the computer is configured to repeatedly integrate the difference between the first angular rate output and the second angular rate output from an initial value.

37. A method of detecting angular motion, the method comprising the steps of:
sensing a first angular rate of a first moving body and producing a first angular rate output;
sensing a second angular rate of a second moving body and producing a second angular rate output; and integrating the difference between the first and second angular rate outputs to produce a first angle signal.

38. The method of claim 37 in which the first moving body is the shank of an animal and the second moving body is the thigh of an animal.

39. The method of claim 38 further comprising the step of operating on stored position data of the first and second bodies and the first angle signal to update the stored position data.

40. The method of claim 38 further comprising the step of repeatedly integrating the difference between the first and second angular rate outputs from a zero initial value to produce a first angle signal.

41. The method of claim 40 in which the zero initial value is obtained by resetting integrators.

42. The method of claim 41 in which the integrators are reset upon the occurrence of an event.

43. A method of detecting angular motion, the method comprising the steps of:
sensing a first angular rate of a first body and producing a first angular rate output;
sensing an initial inclination of a stationary body and producing an initial inclination output;
integrating the first angular rate output to produce a first angle signal representing angular motion of the first body; and summing the first angle signal and the initial inclination output to produce a second angle signal.

44. The method of claim 43 further comprising the steps of:
sensing a second angular rate of a second moving body and producing a second angular rate output; and integrating the difference between the first and second angular rate outputs to produce a third angle signal.

45. The method of claim 43 further comprising operating on stored position data of the first and second bodies and the first, second and third angle signals to update the stored position data.

46. The method of claim 43 in which the first moving body is the shank of an animal and the second moving body is the thigh of an animal.

47. The method of claim 43 in which the first moving body is the shank of an animal and the stationary body is the foot of an animal.

48. The method of claim 43 further comprising the step of repeatedly integrating the first angular rate output from a zero initial value to produce the first angle signal.

49. The method of claim 48 in which the zero initial value is obtained by resetting integrators.

50. The method of claim 49 in which the integrators are reset upon the occurrence of an event.

51. The method of claim 50 in which the event is a repeated occurrence in the movement of an animal.

52. The method of claim 51 in which the repeated occurrence is a stationary condition of a human foot.