# US 2014/0207331 A1 · Estimation of the relative attitude and position between a vehicle body and an implement operably coupled to the vehicle body

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2014/0207331 A1
- Document ID: US20140207331A1
- Status: Published application
- Kind: A1
- Application number: 14/125,361
- Filed: 2012-02-10
- Published: 2014-07-24
- Earliest claimed: 2012-02-10
- Assignees: Topcon Positioning Systems, Inc.
- Inventors: Alexey Andreevich Kosarev; Alexey Vladislavovich Zhdanov
- Web page: https://patentgrep.com/patent/US20140207331A1

## CPC

- E02F 9/264: {Sensors and their calibration for indicating the position of the work tool}
- E02F 3/845: {using mechanical sensors to determine the blade position, e.g. inclinometers, gyroscopes, pendulums}
- G05D 1/0891: {specially adapted for land vehicles}
- G01P 15/00: Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
- E02F: DREDGING; SOIL-SHIFTING (winning peat E21C49/00)
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G01P: MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT (measuring or recording blood flow A61B5/02, A61B8/06; monitoring speed or deceleration of electrically-propelled vehicles B60L3/00; vehicle lighting systems adapted to indicate speed B60Q1/54; determining position or course in navigation, measuring ground distance in geodesy or surveying G01C; combined measuring devices for measuring two or more variables of movement G01C23/00; measuring velocity of sound G01H; measuring velocity of light G01J7/00; measuring direction or velocity of solid objects by reception or emission of radiowaves or other waves and based on propagation effects, e.g. Doppler effect, propagation time, direction of propagation, G01S; measuring speed of nuclear radiation G01T; measuring acceleration of gravity G01V; {measuring or recording the speed of trains B61L23/00; speed indicators incorporated in motor vehicles B60K35/00; measuring frequency or phase G01R; traffic control G08G})

## Abstract

An implement having an implement acceleration is operably coupled to a vehicle body having a body acceleration. An attitude of the implement is estimated by receiving acceleration measurements from an accelerometer mounted on the vehicle body and an accelerometer mounted on the implement. A state vector estimate is calculated based at least in part on the body acceleration measurement and the implement acceleration measurement. The state vector estimate includes a representation of the attitude of the implement relative to the vehicle body. In addition to the accelerometer measurements, angular velocity measurements can be received from a gyro mounted on the vehicle body and a gyro mounted on the implement. The state vector estimate is then calculated based at least in part on the body acceleration measurement, the implement acceleration measurement, the body angular velocity measurement, and the implement angular velocity measurement.

## Claims

### Claim 1

A method for estimating an attitude in a plurality of attitudes of an implement operably coupled to a vehicle body, the implement having an implement acceleration and the vehicle body having a body acceleration, wherein each attitude in the plurality of attitudes corresponds to a time instant in a plurality of time instants, the method comprising the steps of:
receiving a body acceleration measurement from a body accelerometer mounted on the vehicle body;
receiving an implement acceleration measurement from an implement accelerometer mounted on the implement;
calculating a first state vector estimate corresponding to a first time instant in the plurality of time instants, wherein:
the first state vector estimate is based at least in part on the body acceleration measurement and the implement acceleration measurement; and
the first state vector estimate comprises a representation of a first attitude of the implement relative to the vehicle body.

### Claim 2 (depends on 1)

The method of claim 1, further comprising the step of:
calculating an estimate of a position of the implement relative to the vehicle body based at least in part on the representation of the first attitude of the implement relative to the vehicle body.

### Claim 3 (depends on 1)

The method of claim 1, further comprising the step of:
predicting a second state vector estimate corresponding to a second time instant in the plurality of time instants, wherein:
the second state vector estimate is based at least in part on the first state vector estimate and a time-dependent system model; and
the second state vector estimate comprises a representation of a second attitude of the implement relative to the vehicle body.

### Claim 4 (depends on 1)

The method of claim 1, further comprising the steps of:
calculating a measurement vector based at least in part on the body acceleration measurement and the implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 5 (depends on 1)

The method of claim 1, further comprising the steps of:
receiving a body angular velocity measurement from a body gyro mounted on the vehicle body; and
receiving an implement angular velocity measurement from an implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the body angular velocity measurement and the implement angular velocity measurement.

### Claim 6 (depends on 5)

The method of claim 5, further comprising the steps of:
calculating a measurement vector based at least in part on the body acceleration measurement, the implement acceleration measurement, the body angular velocity measurement, and the implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 7 (depends on 1)

The method of claim 1, wherein the body acceleration measurement is a first body acceleration measurement, the body accelerometer is a first body accelerometer, the implement acceleration measurement is a first implement acceleration measurement, and the implement accelerometer is a first implement accelerometer, further comprising the steps of:
receiving a second body acceleration measurement from a second body accelerometer mounted on the vehicle body; and
receiving a second implement acceleration measurement from a second implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the second body acceleration measurement and the second implement acceleration measurement.

### Claim 8 (depends on 7)

The method of claim 7, further comprising the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, and the second implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 9 (depends on 7)

The method of claim 7, further comprising the steps of:
receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement; and
receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement.

### Claim 10 (depends on 9)

The method of claim 9, further comprising the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 11 (depends on 7)

The method of claim 7, further comprising the steps of:
receiving a third body acceleration measurement from a third body accelerometer mounted on the vehicle body; and
receiving a third implement acceleration measurement from a third implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the third body acceleration measurement and the third implement acceleration measurement.

### Claim 12 (depends on 11)

The method of claim 11, further comprising the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, and the third implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 13 (depends on 11)

The method of claim 11, further comprising the steps of:
receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
receiving a third body angular velocity measurement from a third body gyro mounted on the vehicle body;
receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement;
receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement; and
receiving a third implement angular velocity measurement from a third implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement.

### Claim 14 (depends on 13)

The method of claim 13, further comprising the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the third implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 15

An apparatus for estimating an attitude in a plurality of attitudes of an implement operably coupled to a vehicle body, the implement having an implement acceleration and the vehicle body having a body acceleration, wherein each attitude in the plurality of attitudes corresponds to a time instant in a plurality of time instants, the apparatus comprising:
means for receiving a body acceleration measurement from a body accelerometer mounted on the vehicle body;
means for receiving an implement acceleration measurement from an implement accelerometer mounted on the implement;
means for calculating a first state vector estimate corresponding to a first time instant in the plurality of time instants, wherein:
the first state vector estimate is based at least in part on the body acceleration measurement and the implement acceleration measurement; and
the first state vector estimate comprises a representation of a first attitude of the implement relative to the vehicle body.

### Claim 16 (depends on 15)

The apparatus of claim 15, further comprising:
means for calculating an estimate of a position of the implement relative to the vehicle body based at least in part on the representation of the first attitude of the implement relative to the vehicle body.

### Claim 17 (depends on 15)

The apparatus of claim 15, further comprising:
means for predicting a second state vector estimate corresponding to a second time instant in the plurality of time instants, wherein:
the second state vector estimate is based at least in part on the first state vector estimate and a time-dependent system model; and
the second state vector estimate comprises a representation of a second attitude of the implement relative to the vehicle body.

### Claim 18 (depends on 15)

The apparatus of claim 15, further comprising:
means for calculating a measurement vector based at least in part on the body acceleration measurement and the implement acceleration measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 19 (depends on 15)

The apparatus of claim 15, further comprising:
means for receiving a body angular velocity measurement from a body gyro mounted on the vehicle body; and
means for receiving an implement angular velocity measurement from an implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the body angular velocity measurement and the implement angular velocity measurement.

### Claim 20 (depends on 19)

The apparatus of claim 19, further comprising:
means for calculating a measurement vector based at least in part on the body acceleration measurement, the implement acceleration measurement, the body angular velocity measurement, and the implement angular velocity measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 21 (depends on 15)

The apparatus of claim 15, wherein the body acceleration measurement is a first body acceleration measurement, the body accelerometer is a first body accelerometer, the implement acceleration measurement is a first implement acceleration measurement, and the implement accelerometer is a first implement accelerometer, further comprising:
means for receiving a second body acceleration measurement from a second body accelerometer mounted on the vehicle body; and
means for receiving a second implement acceleration measurement from a second implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the second body acceleration measurement and the second implement acceleration measurement.

### Claim 22 (depends on 21)

The apparatus of claim 21, further comprising:
means for calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, and the second implement acceleration measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 23 (depends on 21)

The apparatus of claim 21, further comprising:
means for receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
means for receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
means for receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement; and
means for receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement.

### Claim 24 (depends on 23)

The apparatus of claim 23, further comprising:
means for calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 25 (depends on 21)

The apparatus of claim 21, further comprising:
means for receiving a third body acceleration measurement from a third body accelerometer mounted on the vehicle body; and
means for receiving a third implement acceleration measurement from a third implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the third body acceleration measurement and the third implement acceleration measurement.

### Claim 26 (depends on 25)

The apparatus of claim 25, further comprising:
means for calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, and the third implement acceleration measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 27 (depends on 25)

The apparatus of claim 25, further comprising:
means for receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
means for receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
means for receiving a third body angular velocity measurement from a third body gyro mounted on the vehicle body;
means for receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement;
means for receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement; and
means for receiving a third implement angular velocity measurement from a third implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement.

### Claim 28 (depends on 27)

The apparatus of claim 27, further comprising:
means for calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the third implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement; and
means for calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 29

A computer readable medium storing computer program instructions for estimating an attitude in a plurality of attitudes of an implement operably coupled to a vehicle body, the implement having an implement acceleration and the vehicle body having a body acceleration, wherein each attitude in the plurality of attitudes corresponds to a time instant in a plurality of time instants, the computer program instructions defining the steps of:
receiving a body acceleration measurement from a body accelerometer mounted on the vehicle body;
receiving an implement acceleration measurement from an implement accelerometer mounted on the implement;
calculating a first state vector estimate corresponding to a first time instant in the plurality of time instants, wherein:
the first state vector estimate is based at least in part on the body acceleration measurement and the implement acceleration measurement; and
the first state vector estimate comprises a representation of a first attitude of the implement relative to the vehicle body.

### Claim 30 (depends on 29)

The computer readable medium of claim 29, wherein the computer program instructions further comprise computer program instructions defining the step of:
calculating an estimate of a position of the implement relative to the vehicle body based at least in part on the representation of the first attitude of the implement relative to the vehicle body.

### Claim 31 (depends on 29)

The computer readable medium of claim 29, wherein the computer program instructions further comprise computer program instructions defining the step of:
predicting a second state vector estimate corresponding to a second time instant in the plurality of time instants, wherein:
the second state vector estimate is based at least in part on the first state vector estimate and a time-dependent system model; and
the second state vector estimate comprises a representation of a second attitude of the implement relative to the vehicle body.

### Claim 32 (depends on 29)

The computer readable medium of claim 29, wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the body acceleration measurement and the implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 33 (depends on 29)

The computer readable medium of claim 29, wherein the computer program instructions further comprise computer program instructions defining the steps of:
receiving a body angular velocity measurement from a body gyro mounted on the vehicle body; and
receiving an implement angular velocity measurement from an implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the body angular velocity measurement and the implement angular velocity measurement.

### Claim 34 (depends on 33)

The computer readable medium of claim 33, wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the body acceleration measurement, the implement acceleration measurement, the body angular velocity measurement, and the implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 35 (depends on 29)

The computer readable medium of claim 29, wherein the body acceleration measurement is a first body acceleration measurement, the body accelerometer is a first body accelerometer, the implement acceleration measurement is a first implement acceleration measurement, and the implement accelerometer is a first implement accelerometer, and wherein the computer program instructions further comprise computer program instructions defining the steps of:
receiving a second body acceleration measurement from a second body accelerometer mounted on the vehicle body; and
receiving a second implement acceleration measurement from a second implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the second body acceleration measurement and the second implement acceleration measurement.

### Claim 36 (depends on 35)

The computer readable medium of claim 35, wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, and the second implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 37 (depends on 35)

The computer readable medium of claim 35, wherein the computer program instructions further comprise computer program instructions defining the steps of:
receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement; and
receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement.

### Claim 38 (depends on 37)

The computer readable medium of claim 37, wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the first implement angular velocity measurement, and the second implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 39 (depends on 35)

The computer readable medium of claim 35, wherein the computer program instructions further comprise computer program instructions defining the steps of:
receiving a third body acceleration measurement from a third body accelerometer mounted on the vehicle body; and
receiving a third implement acceleration measurement from a third implement accelerometer mounted on the implement;
wherein the first state vector estimate is further based at least in part on the third body acceleration measurement and the third implement acceleration measurement.

### Claim 40 (depends on 39)

The computer readable medium of claim 39, further wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, and the third implement acceleration measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.

### Claim 41 (depends on 39)

The computer readable medium of claim 39, wherein the computer program instructions further comprise computer program instructions defining the steps of:
receiving a first body angular velocity measurement from a first body gyro mounted on the vehicle body;
receiving a second body angular velocity measurement from a second body gyro mounted on the vehicle body;
receiving a third body angular velocity measurement from a third body gyro mounted on the vehicle body;
receiving a first implement angular velocity measurement from a first implement gyro mounted on the implement;
receiving a second implement angular velocity measurement from a second implement gyro mounted on the implement; and
receiving a third implement angular velocity measurement from a third implement gyro mounted on the implement;
wherein the first state vector estimate is further based at least in part on the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement.

### Claim 42 (depends on 41)

The computer readable medium of claim 41, wherein the computer program instructions further comprise computer program instructions defining the steps of:
calculating a measurement vector based at least in part on the first body acceleration measurement, the second body acceleration measurement, the third body acceleration measurement, the first implement acceleration measurement, the second implement acceleration measurement, the third implement acceleration measurement, the first body angular velocity measurement, the second body angular velocity measurement, the third body angular velocity measurement, the first implement angular velocity measurement, the second implement angular velocity measurement, and the third implement angular velocity measurement; and
calculating the first state vector estimate by performing an extended Kalman filter procedure on the measurement vector.