# US 12,008,173 B2 · Multi-sensor handle controller hybrid tracking method and device

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

- Number: US 12,008,173 B2
- Document ID: US12008173B2
- Status: Granted
- Kind: B2
- Application number: 18/086,425
- Filed: 2022-12-21
- Published: 2023-04-20
- Granted: 2024-06-11
- Earliest claimed: 2020-07-01
- Assignees: Qingdao Pico Technology Co., Ltd.
- Applicants: Qingdao Pico Technology Co., Ltd.
- Inventors: Tao Wu
- Web page: https://patentgrep.com/patent/US12008173B2

## CPC

- G06F 3/0346: with detection of the device orientation or free movement in a three-dimensional [3D] space, e.g. 3D mice, 6-DOF [six degrees of freedom] pointers using gyroscopes, accelerometers or tilt-sensors
- G06T 7/248: {involving reference images or patches}
- G02B 27/0101: {characterised by optical features (G02B27/0172 takes precedence)}
- G02B 27/0172: {characterised by optical features}
- G06F 3/011: {Arrangements for interaction with the human body, e.g. for user immersion in virtual reality (blind teaching G09B21/00)}
- G06F 3/012: {Head tracking input arrangements}
- G06F 3/0304: {Detection arrangements using opto-electronic means (constructional details of pointing devices not related to the detection arrangement using opto-electronic means G06F3/033; optical digitisers G06F3/042)}
- G06T 7/277: involving stochastic approaches, e.g. using Kalman filters
- G06T 7/73: using feature-based methods
- G02B 2027/0138: {comprising image capture systems, e.g. camera}
- G02B 2027/014: {comprising information/image processing systems}
- G06T 2207/20024: Filtering details
- G06T 2207/20221: Image fusion; Image merging
- G06T 2207/30204: Marker
- G06F: ELECTRIC DIGITAL DATA PROCESSING (computer systems based on specific computational models G06N)
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G02B: OPTICAL ELEMENTS, SYSTEMS OR APPARATUS

## Abstract

The present disclosure discloses a multi-sensor handle controller hybrid tracking method and device, wherein the method includes: acquiring tracking data of a handle controller, the tracking data including optical tracking data, electromagnetic tracking data, and inertial navigation data; constructing a state transition model and an observation model of an extended Kalman filtering iteration strategy according to the tracking data, and performing an extended Kalman filtering fusion on the optical tracking data, the electromagnetic tracking data, and the inertial navigation data; and determining position and orientation information of the handle controller in a space according to the extended Kalman filtering iteration strategy.

## Claims

### Claim 1

A multi-sensor handle controller hybrid tracking method, comprising:
acquiring tracking data of a handle controller, the tracking data comprising optical tracking data, electromagnetic tracking data, and inertial navigation data;
constructing a state transition model and an observation model of an extended Kalman filtering iteration strategy according to the tracking data, and performing an extended Kalman filtering fusion on the optical tracking data, the electromagnetic tracking data, and the inertial navigation data, wherein the constructing a state transition model and an observation model of an extended Kalman filtering iteration strategy according to the tracking data further comprises:
acquiring 6DoF data and velocity information of an optical pattern mark point on the handle controller relative to a first camera on a head mounted display in real time according to the optical tracking data,
converting 6DoF data with respect to other cameras into 6DoF data under a coordinate system with the first camera as an origin, by using calibration parameters among the cameras, which is denoted as Poseoptic,
converting electromagnetic 6DoF data of the handle controller into 6DoF data under the coordinate system with the first camera as the origin through calibration parameter of an electromagnetic signal receiver internally provided in the head mounted display and the first camera, which is denoted as PoseEM,
fusing the data Poseoptic and the data PoseEM into new pose data, which is denoted as PoseOEM, and
constructing the state transition model and the observation model of the extended Kalman filtering iteration strategy by using the new pose data; and
determining position and orientation information of the handle controller in a space according to the extended Kalman filtering iteration strategy.

### Claim 2 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the fusing the data Poseoptic and the data PoseEM into the new pose data, which is denoted as PoseOEM comprises:
respectively determining whether an angle difference between the data Poseoptic and the data PoseEM on x axis, y axis, and z axis is less than a first set threshold, and whether a displacement difference therebetween is less than a second set threshold;
if the angle difference is less than the first set threshold and the displacement difference is less than the second set threshold, respectively taking an average of the data Poseoptic and the data PoseEM in the x-axis direction, the y-axis direction, and the z-axis direction as the data PoseOEM; and
if the angle difference is not less than the first set threshold and/or the displacement difference is not less than the second set threshold, respectively performing weighted average on the data Poseoptic and the data PoseEM in the x-axis direction, the y-axis direction, and the z-axis direction to obtain the data PoseOEM.

### Claim 3 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the state transition model of the extended Kalman filtering iteration strategy is as follows:
❘
"\[LeftBracketingBar]"
p
x
k
v
x
k
p
y
k
v
y
k
p
z
k
v
z
k
❘
"\[RightBracketingBar]"
=
[
1
Δ
⁢
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
⁢
T
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
⁢
T
0
0
0
0
0
1
]
*
❘
"\[LeftBracketingBar]"
p
x
k
-
1
v
x
k
-
1
p
y
k
-
1
v
y
k
-
1
p
z
k
-
1
v
z
k
-
1
❘
"\[RightBracketingBar]"
+
[
0.5
*
Δ
⁢
T
2
0
0
Δ
⁢
T
0
0
0
0.5
*
Δ
⁢
T
2
0
0
Δ
⁢
T
0
0
0
0.5
*
Δ
⁢
T
2
0
0
Δ
⁢
T
]
*
❘
"\[LeftBracketingBar]"
Acc
x
k
Acc
y
k
Acc
z
k
❘
"\[RightBracketingBar]"
where pxk and pxk-1 are respectively displacement components in the x-axis direction after optimization iteration at a kth timing and a (k−1)th timing, and pyk and
pyk-1 are respectively displacement components in y-axis direction after the optimization iteration at the kth timing and the (k−1)th timing, and pzk and pzk-1 are respectively displacement components in z-axis direction after the optimization iteration at the kth timing and the (k−1)th timing;
vxk and vxk-1 are respectively velocity displacement components in the x-axis direction in the optical tracking data at the kth timing and at the (k−1)th timing, vyk and
vyk-1 are respectively velocity displacement components in the y-axis direction in the optical tracking data at the kth timing and at the (k−1)th timing, and vzk and vzk-1 are respectively velocity displacement components in the z-axis direction in the optical tracking data at the kth timing and at the (k−1)th timing;
Accxk, Accyk, and Acczk respectively denote motion data of a gravitational acceleration sensor in the x axis, the y axis and the z axis after a gravity direction is removed at the kth timing; and
ΔT denotes a time difference between the kth timing and the (k−1)th timing.

### Claim 4 (depends on 3)

The multi-sensor handle controller hybrid tracking method according to claim 3, wherein, the observation model of the extended Kalman filtering iteration strategy is as follows:
❘
"\[LeftBracketingBar]"
Zp
x
k
Zp
y
k
Zp
z
k
❘
"\[RightBracketingBar]"
=
[
1
0
0
0
1
0
0
0
1
]
*
❘
"\[LeftBracketingBar]"
Pose
x
k
Pose
y
k
Pose
z
k
❘
"\[RightBracketingBar]"
where Zpxk, Zpyk, and Zpzk are respectively position data in the x direction, the y direction, and the z direction that are generated by optimization according to the extended Kalman filtering iteration;
k denotes a timing; and
Posexk, Poseyk, and Posezk are respectively position movement data in the directions of the three axes x, y, and z at the kth timing in the fused data.

### Claim 5 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the method further comprises: setting a process noise covariance matrix as follows:
PNoiseCov
=
[
p_error
0
0
0
0
0
0
v_error
0
0
0
0
0
0
p_error
0
0
0
0
0
0
v_error
0
0
0
0
0
0
p_error
0
0
0
0
0
0
v_error
]
where PNoiseCov denotes the process noise covariance matrix, p_error denotes a displacement noise error, and v_error denotes a velocity noise error.

### Claim 6 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the method further comprises: setting a measurement noise covariance matrix as follows:
MNoiseCov
=
[
M_error
0
0
0
M_error
0
0
0
M_error
]
where MNoiseCov denotes the measurement noise covariance matrix, and
M_error denotes a measurement noise error.

### Claim 7 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the acquiring the optical tracking data of the handle controller comprises:
capturing motion of the handle controller in the space in real time, and obtaining a handle image;
performing feature detection on the handle image, and acquiring a position coordinate of the optical pattern mark point on the handle controller on the handle image; and
solving, by a PNP algorithm, position and orientation data of the optical pattern mark point relative to a camera, as the optical tracking data of the handle controller.

### Claim 8 (depends on 1)

The multi-sensor handle controller hybrid tracking method according to claim 1, wherein, the acquiring the electromagnetic tracking data of the handle controller comprises:
emitting an electromagnetic signal by using an electromagnetic signal generator internally provided in the handle controller;
receiving the electromagnetic signal through an electromagnetic signal receiver internally provided in a head mounted display; and
acquiring, through an electromagnetic positioning six-degree-of-freedom solution model, position and orientation data of the electromagnetic signal generator relative to the electromagnetic signal receiver, as the electromagnetic tracking data of the handle controller.

### Claim 9

A multi-sensor handle controller hybrid tracking device, wherein, a surface of a handle controller is provided with an optical pattern mark point, and the device comprises:
a plurality of cameras provided on a head mounted display and configured to track and shoot the optical pattern mark point;
an optical sensor configured to acquire optical tracking data of the optical pattern mark point;
an electromagnetic sensor configured to acquire electromagnetic tracking data of the handle controller;
an inertial navigation sensor configured to acquire inertial navigation data of the handle controller;
the head mounted display internally provided with a wireless transmission module and configured to receive the optical tracking data, the electromagnetic tracking data, and the inertial navigation data;
a model construction module configured to construct a state transition model and an observation model of an extended Kalman filtering iteration strategy and perform an extended Kalman filtering fusion on the optical tracking data, the electromagnetic tracking data, and the inertial navigation data, wherein constructing a state transition model and an observation model of an extended Kalman filtering iteration strategy according to the tracking data further comprises:
acquiring 6DoF data and velocity information of an optical pattern mark point on the handle controller relative to a first camera on a head mounted display in real time according to the optical tracking data,
converting 6DoF data with respect to other cameras into 6DoF data under a coordinate system with the first camera as an origin, by using calibration parameters among the cameras, which is denoted as Poseoptic,
converting electromagnetic 6DoF data of the handle controller into 6DoF data under the coordinate system with the first camera as the origin through calibration parameter of an electromagnetic signal receiver internally provided in the head mounted display and the first camera, which is denoted as PoseEM,
fusing the data and the data Poseoptic and the data PoseEM into the new pose data, which is denoted as PoseOEM, and
constructing the state transition model and the observation model of the extended Kalman filtering iteration strategy by using the new pose data; and
a tracking module configured to determine position and orientation information of the handle controller in a space according to the extended Kalman filtering iteration strategy.