Digital signal processing method and system thereof for precision orientation measurements
First Claim
1. A digital signal processing system for orientation measurements of a body frame, comprising:
- an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earths magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module.
1 Assignment
0 Petitions
Accused Products
Abstract
The present invention provides a digital signal processing method and system thereof for producing precision platform orientation measurements and local Earth'"'"'s magnetic measurements by measuring threes axes gravity acceleration digital signals by an acceleration producer, detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve digital three-axes Earth'"'"'s magnetic field vector signals, and producing pitch, roll, and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth magnetic field vector signals by a Digital Signal Processor (DSP) chipset.
177 Citations
36 Claims
-
1. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earths magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (2, 3)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module; said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface.
-
-
4. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame, wherein said acceleration producer comprises three MEMS accelerometers, which are orthogonally installed to achieve orthogonal three-axes gravity acceleration measurements;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (5, 6)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module; said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface.
-
-
7. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame, wherein said acceleration producer comprises three MEMS accelerometers, which are orthogonally installed to achieve orthogonal three-axes gravity acceleration measurements;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said Earth'"'"'s magnetic field detector is a device for measuring said Earth'"'"'s magnetic field vector, including a fluxgate, magnetoresistance (MR) sensor, and magnetoinductive sensors;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earths magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (8, 9)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module; said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface.
-
-
10. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
a velocity producer providing platform velocity measurements expressed in said body frame so as to produce a motion acceleration of said body frame to input into said acceleration producer; and
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (11, 12)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module;
said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface;
said level-plane motion acceleration estimation module receiving said platform velocity measurements expressed in said body frame from said velocity producer and an attitude matrix from said attitude matrix computation module, wherein said attitude matrix represents a transformation from said body frame to said level-plane frame, and producing said motion acceleration which is an estimated level-plane motion acceleration input to said digital torquer module, wherein said level-plane motion acceleration in said level-plane gravity acceleration from said first digital low pass filter and said second digital low-pass filter is removed using said estimated level-plane motion acceleration from said level-plane motion acceleration estimation module.
-
-
13. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame, wherein said acceleration producer comprises three MEMS accelerometers, which are orthogonally installed to achieve orthogonal three-axes gravity acceleration measurements;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
a velocity producer providing platform velocity measurements expressed in said body frame so as to produce a motion acceleration of said body frame to input into said acceleration producer; and
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (14, 15)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module;
said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface;
said level-plane motion acceleration estimation module receiving said platform velocity measurements expressed in said body frame from said velocity producer and an attitude matrix from said attitude matrix computation module, wherein said attitude matrix represents a transformation from said body frame to said level-plane frame, and producing said motion acceleration which is an estimated level-plane motion acceleration input to said digital torquer module, wherein said level-plane motion acceleration in said level-plane gravity acceleration from said first digital low pass filter and said second digital low-pass filter is removed using said estimated level-plane motion acceleration from said level-plane motion acceleration estimation module.
-
-
16. A digital signal processing system for orientation measurements of a body frame, comprising:
-
an acceleration producer, measuring gravity acceleration analog signals in orthogonal three-axes expressed in said body frame, wherein said acceleration producer comprises three MEMS accelerometers, which are orthogonally installed to achieve orthogonal three-axes gravity acceleration measurements;
a conditioning and analog/digital converting circuitry, suppressing noises of said gravity acceleration analog signals and digitizing said gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
a Digital Signal Processor (DSP) chipset, which is interfaced with said conditioning and analog/digital converting circuitry, receiving said three-axes gravity acceleration digital signals and producing pitch and roll angle using DSP algorithms;
a user interface, which is connected with said DSP chipset, for displaying, inputting and outputting measurement data;
a velocity producer providing platform velocity measurements expressed in said body frame so as to produce a motion acceleration of said body frame to input into said acceleration producer; and
an Earth'"'"'s Magnetic Field (EMF) detector, detecting Earth'"'"'s magnetic field analog signals in three-axes expressed in said body frame; and
an EMF conditioning and analog/digital converting circuitry, suppressing noise of said Earth'"'"'s magnetic field analog signals and digitizing said Earth'"'"'s magnetic field analog signals to form three-axes Earth'"'"'s magnetic field digital signals;
wherein said DSP chipset is further interfaced with said EMF conditioning and analog/digital converting circuitry to receive a three-axes digital Earth'"'"'s magnetic field vector and produce attitude and heading measurements using said DSP algorithms;
wherein said Earth'"'"'s magnetic field detector is a device for measuring said Earth'"'"'s magnetic field vector, including a fluxgate, magnetoresistance (MR) sensor, and magnetoinductive;
wherein said conditioning and analog/digital converting circuitry, which is connected between said acceleration producer and said DSP chipset, substantially acquires said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
amplifies said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
converts said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
provides data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein said DSP chipset comprises a first pre-processing module, an initial pitch and roll estimation module, a level-plane gravity acceleration computation module, a pitch and roll refinement loop closure module, a second pre-processing module, a magnetic field vector error compensation module, and a magnetic heading estimation module, said first pre-processing module smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate, and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve and output smoothed and compensated three-axes gravity acceleration digital signals to said initial pitch and roll estimation module and said level-plane gravity acceleration computation module;
said initial pitch and roll estimation module receiving said smoothed and compensated three-axes gravity acceleration digital signals from said first pre-processing module and running one time initially to provide rough pitch and roll angle estimates;
said level-plane gravity acceleration computation module receiving said smoothed three-axes gravity acceleration digital signals and a transform matrix from said body frame to a level-plane frame from said pitch and roll refinement loop closure module, and transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in said level-plane frame;
said pitch and roll refinement loop closure module receiving said rough pitch and roll angle from said initial pitch and roll estimation module, level-plane plane gravity acceleration components from said level-plane gravity acceleration computation module, and heading angle from said magnetic heading estimation module, so as to refine said rough pitch and roll angles;
said second pre-processing module smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, wherein said smoothed three-axes Earth'"'"'s magnetic digital signals are output to said magnetic field vector error compensation module;
said magnetic field vector error compensation module performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials;
said magnetic heading estimation module receiving said three-axes digital Earth'"'"'s magnetic field signals from said error compensation module of said magnetic field vector and said pitch and roll angle data from said pitch and roll refinement loop closure module to estimate an optimal heading angle, which is output to said pitch and roll refinement loop closure module. - View Dependent Claims (17, 18)
said first digital low pass filter receiving a X component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data which is output to said digital torquer module;
said second digital low pass filter receiving a Y component of said level-plane gravity acceleration data from said level-plane gravity acceleration computation module to reject high frequency noises of said Y component of said level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data which is output to said digital torquer module;
said rotation vector generation module receiving said rough pitch and roll angle from said initial pitch and roll estimation module to form a rotation vector representing said rotation motion of said body frame;
said digital torquer module forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
said rotation vector updating module updating said rotation vector using said set of torquer rates, wherein said updated rotation vector is output to said attitude matrix computation module;
said attitude matrix computation module computing said transform matrix using said input updated rotation vector;
wherein said transform matrix is fed back to said level-plane gravity acceleration computation module and said pitch and roll extraction module;
said pitch and roll extraction module extracting said pitch and roll angle using said transform matrix, outputting said pitch and roll angle to said magnetic heading estimation module and said user interface;
said level-plane motion acceleration estimation module receiving said platform velocity measurements expressed in said body frame from said velocity producer and an attitude matrix from said attitude matrix computation module, wherein said attitude matrix represents a transformation from said body frame to said level-plane frame, and producing said motion acceleration which is an estimated level-plane motion acceleration input to said digital torquer module, wherein said level-plane motion acceleration in said level-plane gravity acceleration from said first digital low pass filter and said second digital low-pass filter is removed using said estimated level-plane motion acceleration from said level-plane motion acceleration estimation module.
-
-
19. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals; and
(e) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (e) further comprises the steps of;
(e-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(e-2) running one time initially to provide estimated pitch and roll angles;
(e-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(e-4) refining said estimated pitch and roll angles;
(e-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(e-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(e-7) producing an optimal heading angle;
wherein the step (e-7) further comprises the steps of;
(e-7-1) forming a transformation matrix from said body frame to a level-plane frame, (e-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (e-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (20)
(e-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(e-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(e-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(e-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(e-4-6) updating said rotation vector using said set of torquer rates;
(e-4-7) computing said transform matrix using said input updated rotation vector; and
(e-4-8) extracting pitch and roll angles using said transform matrix.
-
-
21. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals;
(e) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer; and
(f) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (f) further comprises the steps of;
(f-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(f-2) running one time initially to provide estimated pitch and roll angles;
(f-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(f-4) refining said estimated pitch and roll angles;
(f-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(f-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(f-7) producing an optimal heading angle;
wherein the step (f-7) further comprises the steps of;
(f-7-1) forming a transformation matrix from said body frame to a level-plane frame, (f-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (f-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (22)
(f-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(f-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(f-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(f-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(f-4-6) updating said rotation vector using said set of torquer rates;
(f-4-7) computing said transform matrix using said input updated rotation vector;
(f-4-8) extracting pitch and roll angles using said transform matrix; and
(f-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
-
23. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earths magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals; and
(e) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (b) further comprises the steps of;
(b-1) acquiring said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
(b-2) amplifying said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
(b-3) converting said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
(b-4) providing data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein the step (e) further comprises the steps of;
(e-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(e-2) running one time initially to provide estimated pitch and roll angles;
(e-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(e-4) refining said estimated pitch and roll angles;
(e-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(e-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(e-7) producing an optimal heading angle;
wherein the step (e-7) further comprises the steps of;
(e-7-1) forming a transformation matrix from said body frame to a level-plane frame, (e-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (e-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (24)
(e-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(e-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(e-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(e-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(e-4-6) updating said rotation vector using said set of torquer rates;
(e-4-7) computing said transform matrix using said input updated rotation vector; and
(e-4-8) extracting pitch and roll angles using said transform matrix.
-
-
25. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals;
(e) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer; and
(f) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (b) further comprises the steps of;
(b-1) acquiring said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
(b-2) amplifying said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
(b-3) converting said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
(b-4) providing data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein the step (f) further comprises the steps of;
(f-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(f-2) running one time initially to provide estimated pitch and roll angles;
(f-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(f-4) refining said estimated pitch and roll angles;
(f-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(f-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(f-7) producing an optimal heading angle;
wherein the step (e-7) further comprises the steps of;
(f-7-1) forming a transformation matrix from said body frame to a level-plane frame, (f-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (f-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (26)
(f-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(f-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(f-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(f-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(f-4-6) updating said rotation vector using said set of torquer rates;
(f-4-7) computing said transform matrix using said input updated rotation vector;
(f-4-8) extracting pitch and roll angles using said transform matrix; and
(f-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
-
27. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals; and
(e) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (e) further comprises the steps of;
(e-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(e-2) running one time initially to provide estimated pitch and roll angles;
(e-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(e-4) refining said estimated pitch and roll angles;
(e-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(e-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(e-7) producing an optimal heading angle;
wherein the step (e-4) further comprises the steps of;
(e-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(e-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(e-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(e-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(e-4-6) updating said rotation vector using said set of torquer rates;
(e-4-7) computing said transform matrix using said input updated rotation vector; and
(e-4-8) extracting pitch and roll angles using said transform matrix.
-
-
28. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals; and
(e) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (b) further comprises the steps of;
(b-1) acquiring said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
(b-2) amplifying said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
(b-3) converting said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
(b-4) providing data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein the step (e) further comprises the steps of;
(e-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(e-2) running one time initially to provide estimated pitch and roll angles;
(e-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(e-4) refining said estimated pitch and roll angles;
(e-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(e-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(e-7) producing an optimal heading angle;
wherein the step (e-4) further comprises the steps of;
(e-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(e-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(e-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(e-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(e-4-6) updating said rotation vector using said set of torquer rates;
(e-4-7) computing said transform matrix using said input updated rotation vector; and
(e-4-8) extracting pitch and roll angles using said transform matrix.
-
-
29. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals;
(e) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer; and
(f) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (f) further comprises the steps of;
(f-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(f-2) running one time initially to provide estimated pitch and roll angles;
(f-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(f-4) refining said estimated pitch and roll angles;
(f-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(f-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(f-7) producing an optimal heading angle;
wherein the step (f-4) further comprises the steps of;
(f-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(f-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(f-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(f-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(f-4-6) updating said rotation vector using said set of torquer rates;
(f-4-7) computing said transform matrix using said input updated rotation vector;
(f-4-8) extracting pitch and roll angles using said transform matrix; and
(f-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
-
30. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring three-axes gravity acceleration analog signals by an acceleration producer;
(b) suppressing noises of said three-axes gravity acceleration analog signals and digitizing said three-axes gravity acceleration analog signals to form three-axes gravity acceleration digital signals;
(c) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve analog three-axes Earth'"'"'s magnetic field vector signals;
(d) digitizing said analog three-axes Earth'"'"'s magnetic field vector signals to form digital three-axes Earth'"'"'s magnetic field vector signals; and
(e) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer;
(f) producing pitch, roll and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth'"'"'s magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (b) further comprises the steps of;
(b-1) acquiring said gravity acceleration analog signals, which are proportional to an Earth'"'"'s gravity field, from said acceleration producer;
(b-2) amplifying said gravity acceleration analog signals to suppress said noises in said gravity acceleration analog signal to form amplified gravity acceleration signals, wherein said noises are signals of said gravity acceleration analog signals not proportional to said Earth'"'"'s gravity field;
(b-3) converting said amplified gravity acceleration signals to form said three-axes gravity acceleration digital signals which are input to said DSP chipset; and
(b-4) providing data/control/address bus connection with said DSP chipset so as to produce an address decode function to enable said DSP chipset to access said acceleration producer and pickup said three-axes gravity acceleration signals;
wherein the step (f) further comprises the steps of;
(f-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(f-2) running one time initially to provide estimated pitch and roll angles;
(f-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame (f-4) refining said estimated pitch and roll angles;
(f-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(f-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(f-7) producing an optimal heading angle;
wherein the step (f-4) further comprises the steps of;
(f-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(f-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(f-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(f-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(f-4-6) updating said rotation vector using said set of torquer rates;
(f-4-7) computing said transform matrix using said input updated rotation vector;
(f-4-8) extracting pitch and roll angles using said transform matrix; and
(f-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
-
31. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring threes axes gravity acceleration digital signals by an acceleration producer;
(b) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve digital three-axes Earth'"'"'s magnetic field vector signals; and
(c) producing pitch, roll, and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (c) further comprises the steps of;
(c-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(c-2) running one time initially to provide estimated pitch and roll angles;
(c-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(c-4) refining said estimated pitch and roll angles;
(c-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(c-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(c-7) producing an optimal heading angle;
wherein the step (c-7) further comprises the steps of;
(c-7-1) forming a transformation matrix from said body frame to a level-plane frame, (c-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (c-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (32)
(c-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(c-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(c-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(c-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(c-4-6) updating said rotation vector using said set of torquer rates;
(c-4-7) computing said transform matrix using said input updated rotation vector, and (c-4-8) extracting pitch and roll angles using said transform matrix.
-
-
33. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring threes axes gravity acceleration digital signals by an acceleration producer;
(b) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve digital three-axes Earth'"'"'s magnetic field vector signals; and
(c) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer;
(d) producing pitch, roll, and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (d) further comprises the steps of;
(d-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(d-2) running one time initially to provide estimated pitch and roll angles;
(d-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(d-4) refining said estimated pitch and roll angles;
(d-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(d-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(d-7) producing an optimal heading angle;
wherein the step (d-7) further comprises the steps of;
(d-7-1) forming a transformation matrix from said body frame to a level-plane frame, (d-7-2) transforming said Earth'"'"'s magnetic vector from said body frame to said level-plane frame to form a measurement vector, which is expressed in said level-plane frame, and (d-7-3) estimating magnetic heading data using said measurement vector expressed in said level-plane frame. - View Dependent Claims (34)
(d-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(d-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(d-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(d-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(d-4-6) updating said rotation vector using said set of torquer rates;
(d-4-7) computing said transform matrix using said input updated rotation vector;
(d-4-8) extracting pitch and roll angles using said transform matrix; and
(d-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
-
35. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring threes axes gravity acceleration digital signals by an acceleration producer;
(b) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve digital three-axes Earth'"'"'s magnetic field vector signals; and
(c) producing pitch, roll, and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (c) further comprises the steps of;
(c-2) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(c-2) running one time initially to provide estimated pitch and roll angles;
(c-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(c-4) refining said estimated pitch and roll angles;
(c-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(c-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(c-7) producing an optimal heading angle;
wherein the step (c-4) further comprises the steps of;
(c-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(c-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(c-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(c-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(c-4-6) updating said rotation vector using said set of torquer rates;
(c-4-7) computing said transform matrix using said input updated rotation vector; and
(c-4-8) extracting pitch and roll angles using said transform matrix.
-
-
36. A digital signal processing method for orientation measurements of a body frame, comprising the steps of:
-
(a) measuring threes axes gravity acceleration digital signals by an acceleration producer;
(b) detecting Earth'"'"'s magnetic field vector measurement by an Earth'"'"'s magnetic field detector to achieve digital three-axes Earth'"'"'s magnetic field vector signals; and
(c) providing platform velocity measurements expressed in said body frame by a velocity producer for producing a motion acceleration of said body frame and inputting into said acceleration producer;
(d) producing pitch, roll, and heading angles using said three-axes gravity acceleration digital signals and said digital three-axes Earth magnetic field vector signals by a Digital Signal Processor (DSP) chipset;
wherein the step (d) further comprises the steps of;
(d-1) smoothing said three-axes gravity acceleration digital signals expressed in said body frame at high sampling rate and compensating errors in said three-axes gravity acceleration digital signals with calibration parameters, including scale factor, bias and misalignment, so as to achieve smoothed and compensated three-axes gravity acceleration digital signals;
(d-2) running one time initially to provide estimated pitch and roll angles;
(d-3) transforming said smoothed three-axes gravity acceleration digital signals into gravity acceleration data expressed in a level-plane frame;
(d-4) refining said estimated pitch and roll angles;
(d-5) smoothing said three-axes digital Earth'"'"'s magnetic signals at high sampling rate, which are expressed in said body frame, to achieve smoothed three-axes Earth'"'"'s magnetic digital signals;
(d-6) performing a compensation procedure using calibration parameters, including scale factors, misalignment parameters, and effects of nearby ferrous materials; and
(d-7) producing an optimal heading angle;
wherein the step (d-4) further comprises the steps of;
(d-4-1) receiving a X component of level-plane gravity acceleration data to reject high frequency noises of said X component of said level-plane gravity acceleration data to obtain a filtered X component of said level-plane gravity acceleration data;
(d-4-2) receiving a Y component of said level-plane gravity acceleration data to reject high frequency noises of said Y component of level-plane gravity acceleration data to obtain a filtered Y component of said level-plane gravity acceleration data;
(d-4-3) receiving said estimated pitch and roll angles to form a rotation vector representing a rotation motion of said body frame;
(d-4-5) forming a set of torquer rates using said X and Y components of said level-plane gravity acceleration data;
(d-4-6) updating said rotation vector using said set of torquer rates;
(d-4-7) computing said transform matrix using said input updated rotation vector;
(d-4-8) extracting pitch and roll angles using said transform matrix; and
(d-4-9) removing said motion acceleration from said level-plane gravity acceleration data.
-
Specification