US 2024/0175685 A1Published application

Weight and reference quaternoin correction unscented quaternoin estimation method

AssigneeHangzhou City University

InventorsZhenbing Qiu, Yanjun Li

Life of this patent

Estimating the term…

Abstract

The invention relates to a weight and reference quaternion correction unscented quaternion estimation method, comprising: obtaining measurement data as quantity measurement through a gyro and a star sensor; establishing a quaternion-based discrete spacecraft nonlinear state space model; estimating an error quaternion, a gyro drift and a corresponding error covariance at a k moment by using an unscented quaternion estimator based on parameter and reference quaternion correction at a k−1 moment; and setting a filtering time as Ntime, if k<Ntime, then repeating the step 3, if k=Ntime, then finishing filtering, and outputting the attitude quaternion, the gyro drift and the corresponding error covariance.

Claims5

Dependent claims

A weight and reference quaternion correction unscented quaternion estimation method, comprising:

step 1:
obtaining measurement data as quantity measurement through a gyro and a star sensor;
step 2:
establishing a quaternion-based discrete spacecraft nonlinear state space model ;
step 3:
estimating an error quaternion, a gyro drift and a corresponding error covariance at a k moment by using an unscented quaternion estimator based on parameter and reference quaternion correction at a k−1 moment;
and
step 4:
setting a filtering time as Ntime, if k<Ntime, then repeating the step 3, if k=Ntime, then finishing filtering, and outputting the attitude quaternion, the gyro drift and the corresponding error covariance;
the step 2 comprises:
step 2.1:
establishing a discrete quaternion state motion equation of a spacecraft;
q
^
k
❘
k
=
1
=
Ω
[
ω
^
k
-
1
]
⁢
q
^
k
-
1
Formula
⁢
(
1
)
Ω
[
ω
^
k
-
1
]
=
[
cos
⁡
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
I
3
×
3
-
[
ψ
k
-
1
×
]
ψ
k
-
1
-
ψ
k
-
1
T
cos
⁢
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
I
3
×
3
]
ψ
k
-
1
=
sin
⁡
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
ω
^
k
-
1

ω
^
k
-
1

wherein q=[q1 q2 q3 q4]T represents a quaternion vector, Ω[⋅] and ψk−1 represent function symbols at the k−1 moment, ω=[ω1 ω2 ω3]T represents a gyro three-axis angular velocity output vector, {circumflex over (ω)}k−1 represents an angular velocity estimation value at the k−1 moment, {circumflex over (q)}k−1 represents an estimation value of an attitude quaternion q at the k−1 moment, I3×3 represents an unit matrix of 3×3, Δt represents a gyro sampling interval, ∥⋅∥ represents a vector norm, ψk−1T represents transposition of ψk−1, and [ψk−1×] represents an antisymmetry matrix of [ψk−1×];
step 2.2:
establishing a discrete angular velocity measurement model:
ω
~
k
=
ω
k
+
1
2
[
β
k
+
β
k
-
1
]
+
[
σ
v
2
Δ
⁢
t
+
1
1
⁢
2
⁢
σ
u
2
⁢
Δ
⁢
t
]
1
/
2
⁢
N
v
Formula
⁢
(
2
)
β
k
=
β
k
-
1
+
σ
u
⁢
Δ
⁢
t
1
/
2
⁢
N
u
Formula
⁢
(
3
)
wherein {tilde over (ω)}k represents a gyro output value at the k moment, ωk represents a real gyro value at the k moment, βk represents a gyro drift at the k moment, σv2 and σu2 represents mean square errors of a gyro measurement noise and a drift noise, Nv and Nu represent a Gaussian white noise with a mean value of zero and a unit array with a variance of three dimensions;
step 2.3:
establishing a star sensor observation model:
z
k
=
[
A
⁡
(
q
)
⁢
r
1
A
⁡
(
q
)
⁢
r
2
⋮
A
⁡
(
q
)
⁢
r
L
]
k
+
[
v
1
v
2
⋮
v
L
]
k
Formula
⁢
(
4
)
wherein zk represents a quantity measurement, ri represent an ith reference vector, i=1, . . . , L, L represents a number of fixed stars observed by the star sensor;
vi represents a zero-mean Gaussian white noise, with a covariance of σi2I3×3, while all vi covariances constitute a measurement variance Rk;
and A(q) represents an attitude matrix, which is defined as follows:
A(q)=(q42−pTp)I3×3+2ppT−2q4[p×] Formula (5)
wherein p=[q1 q2 q3]T represents a quaternion vector part, and [p×] represents an antisymmetric matrix of p;
step 3.1:
calculating a sigma point and a corresponding weight at the k−1 moment, and calculating a quaternion to be propagated:
step 3.2:
updating the quaternion by using the discrete quaternion motion equation;
step 3.3:
calculating the reference quaternion according to the following equation:
N(q1,k|k−1, . . . , q2n,k|k−1){circumflex over (q)}i,k|k−1=λ{circumflex over (q)}i,k|k−1, i=0, . . . ,2n Formula (6)
wherein
N
⁡
(
q
1
,
k
|
k
-
1
,
…
,
q
2
⁢
n
,
k
|
k
-
1
)
=
Σ
i
=
1
2
⁢
n
⁢
Z
⁡
(
q
i
,
k
|
k
-
1
)
⁢
W
i
⁢
Z
T
(
q
i
,
k
|
k
-
1
)
,
Z
⁡
(
q
i
Reject
⁢
k
|
k
-
1
)
=
[
q
4
i
,
k
❘
k
-
1
⁢
I
3
×
3
[
ρ
i
,
k
❘
k
-
1
×
]
-
ρ
i
,
k
❘
k
-
1
T
]
⁢
and
⁢
q
4
i
,
k
❘
k
-
1
represent values of a quaternion scalar part predicted at the k−1 moment corresponding to the ith sigma point,
W
i
=
I
3
×
3
2
⁢
n
,
λ refers to a Lagrangian multiplicative factor, and an eigenvector corresponding to the minimum eigenvalue in a solution of equation is set as a reference quaternion {circumflex over (q)}k|k−1;
step 3.4:
propagating the gyro drift:
Xi,k|k−1β=Xi,k−1β, i=0,1, . . . ,2n Formula (7)
step 3.5:
estimating one-step prediction state and a corresponding error covariance as follows:
{circumflex over (x)}k|k−1=Σi=02nwimXi,k|k−1 Formula (8)
Pk|k−1=Σi=02nwic(Xi,k|k−1−{circumflex over (x)}k|k−1)(Xi,k|k−1−{circumflex over (x)}k|k−1)T+Qk−1 Formula (9)
wherein both wim and wic represent the weights;
step 3.6:
updating the measurement;
step 3.7:
calculating a filtering gain Kk, a state vector {circumflex over (x)}k and a corresponding error covariance Pk at the k moment:
Kk=Pxz,k|k−1Pzz,k|k−1−1 Formula (10)
{circumflex over (x)}k={circumflex over (x)}k|k−1+Kk(zk−{circumflex over (z)}k|k−1) Formula (11)
Pk=Pk|k−1−KkPzz,k|k−1KkT Formula (12)
wherein {circumflex over (x)}k|k−1 represents a state one-step prediction amount of at the k−1 moment, zk represents an observation vector at the k moment, Pk|k−1 represents a one-step prediction covariance, {circumflex over (z)}k|k−1 represents one-step measurement prediction, Pzz,k|k−1 represents a one-step measurement prediction variance, and Pzz,k|k−1 represents a cross covariance;
step 3.8:
updating the quaternion:
{circumflex over (x)}k=[δ{circumflex over (p)}kT{circumflex over (β)}kT]T Formula (13)
wherein {circumflex over (β)}kT represents the transposition of a gyro drift estimation value at the k moment, and δ(⋅) represents an error;
and step 3.9:
resetting δ{circumflex over (p)}k as a zero vector to prepare for a next filtering cycle.

444 more elements

Dependencies are inferred from the claim text.

Facts

Application: 18/426,210Filed 29 Jan 2024
Priority: CN 202211423293.515 Nov 2022
Published: 30 May 2024
Term ends: Not estimatedThe source records don’t support an estimate.

Classification8 codes

G01C 21/025
By astronomical means: with the use of startrackers
G01C 21/20
Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00: instruments for performing navigational calculations
B64G 1/244
Guiding or controlling apparatus, e.g. for attitude control: spacecraft control systems
B64G 1/245
Spacecraft control systems: attitude control algorithms for spacecraft attitude control

Family

Application18/426,210Continuation
US 12,078,489 B2Related patent
US 2024/0175685 A1This patent

From the US filing history. Foreign family members aren’t listed.

Timeline

Citations

Open elsewhere

patentgrep

Loading patent…

Abstract

A weight and reference quaternion correction unscented quaternion estimation method, comprising:

step 1:
obtaining measurement data as quantity measurement through a gyro and a star sensor;
step 2:
establishing a quaternion-based discrete spacecraft nonlinear state space model ;
step 3:
estimating an error quaternion, a gyro drift and a corresponding error covariance at a k moment by using an unscented quaternion estimator based on parameter and reference quaternion correction at a k−1 moment;
and
step 4:
setting a filtering time as Ntime, if k<Ntime, then repeating the step 3, if k=Ntime, then finishing filtering, and outputting the attitude quaternion, the gyro drift and the corresponding error covariance;
the step 2 comprises:
step 2.1:
establishing a discrete quaternion state motion equation of a spacecraft;
q
^
k
❘
k
=
1
=
Ω
[
ω
^
k
-
1
]
⁢
q
^
k
-
1
Formula
⁢
(
1
)
Ω
[
ω
^
k
-
1
]
=
[
cos
⁡
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
I
3
×
3
-
[
ψ
k
-
1
×
]
ψ
k
-
1
-
ψ
k
-
1
T
cos
⁢
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
I
3
×
3
]
ψ
k
-
1
=
sin
⁡
(
0.5

ω
^
k
-
1

⁢
Δ
⁢
t
)
⁢
ω
^
k
-
1

ω
^
k
-
1

wherein q=[q1 q2 q3 q4]T represents a quaternion vector, Ω[⋅] and ψk−1 represent function symbols at the k−1 moment, ω=[ω1 ω2 ω3]T represents a gyro three-axis angular velocity output vector, {circumflex over (ω)}k−1 represents an angular velocity estimation value at the k−1 moment, {circumflex over (q)}k−1 represents an estimation value of an attitude quaternion q at the k−1 moment, I3×3 represents an unit matrix of 3×3, Δt represents a gyro sampling interval, ∥⋅∥ represents a vector norm, ψk−1T represents transposition of ψk−1, and [ψk−1×] represents an antisymmetry matrix of [ψk−1×];
step 2.2:
establishing a discrete angular velocity measurement model:
ω
~
k
=
ω
k
+
1
2
[
β
k
+
β
k
-
1
]
+
[
σ
v
2
Δ
⁢
t
+
1
1
⁢
2
⁢
σ
u
2
⁢
Δ
⁢
t
]
1
/
2
⁢
N
v
Formula
⁢
(
2
)
β
k
=
β
k
-
1
+
σ
u
⁢
Δ
⁢
t
1
/
2
⁢
N
u
Formula
⁢
(
3
)
wherein {tilde over (ω)}k represents a gyro output value at the k moment, ωk represents a real gyro value at the k moment, βk represents a gyro drift at the k moment, σv2 and σu2 represents mean square errors of a gyro measurement noise and a drift noise, Nv and Nu represent a Gaussian white noise with a mean value of zero and a unit array with a variance of three dimensions;
step 2.3:
establishing a star sensor observation model:
z
k
=
[
A
⁡
(
q
)
⁢
r
1
A
⁡
(
q
)
⁢
r
2
⋮
A
⁡
(
q
)
⁢
r
L
]
k
+
[
v
1
v
2
⋮
v
L
]
k
Formula
⁢
(
4
)
wherein zk represents a quantity measurement, ri represent an ith reference vector, i=1, . . . , L, L represents a number of fixed stars observed by the star sensor;
vi represents a zero-mean Gaussian white noise, with a covariance of σi2I3×3, while all vi covariances constitute a measurement variance Rk;
and A(q) represents an attitude matrix, which is defined as follows:
A(q)=(q42−pTp)I3×3+2ppT−2q4[p×] Formula (5)
wherein p=[q1 q2 q3]T represents a quaternion vector part, and [p×] represents an antisymmetric matrix of p;
step 3.1:
calculating a sigma point and a corresponding weight at the k−1 moment, and calculating a quaternion to be propagated:
step 3.2:
updating the quaternion by using the discrete quaternion motion equation;
step 3.3:
calculating the reference quaternion according to the following equation:
N(q1,k|k−1, . . . , q2n,k|k−1){circumflex over (q)}i,k|k−1=λ{circumflex over (q)}i,k|k−1, i=0, . . . ,2n Formula (6)
wherein
N
⁡
(
q
1
,
k
|
k
-
1
,
…
,
q
2
⁢
n
,
k
|
k
-
1
)
=
Σ
i
=
1
2
⁢
n
⁢
Z
⁡
(
q
i
,
k
|
k
-
1
)
⁢
W
i
⁢
Z
T
(
q
i
,
k
|
k
-
1
)
,
Z
⁡
(
q
i
Reject
⁢
k
|
k
-
1
)
=
[
q
4
i
,
k
❘
k
-
1
⁢
I
3
×
3
[
ρ
i
,
k
❘
k
-
1
×
]
-
ρ
i
,
k
❘
k
-
1
T
]
⁢
and
⁢
q
4
i
,
k
❘
k
-
1
represent values of a quaternion scalar part predicted at the k−1 moment corresponding to the ith sigma point,
W
i
=
I
3
×
3
2
⁢
n
,
λ refers to a Lagrangian multiplicative factor, and an eigenvector corresponding to the minimum eigenvalue in a solution of equation is set as a reference quaternion {circumflex over (q)}k|k−1;
step 3.4:
propagating the gyro drift:
Xi,k|k−1β=Xi,k−1β, i=0,1, . . . ,2n Formula (7)
step 3.5:
estimating one-step prediction state and a corresponding error covariance as follows:
{circumflex over (x)}k|k−1=Σi=02nwimXi,k|k−1 Formula (8)
Pk|k−1=Σi=02nwic(Xi,k|k−1−{circumflex over (x)}k|k−1)(Xi,k|k−1−{circumflex over (x)}k|k−1)T+Qk−1 Formula (9)
wherein both wim and wic represent the weights;
step 3.6:
updating the measurement;
step 3.7:
calculating a filtering gain Kk, a state vector {circumflex over (x)}k and a corresponding error covariance Pk at the k moment:
Kk=Pxz,k|k−1Pzz,k|k−1−1 Formula (10)
{circumflex over (x)}k={circumflex over (x)}k|k−1+Kk(zk−{circumflex over (z)}k|k−1) Formula (11)
Pk=Pk|k−1−KkPzz,k|k−1KkT Formula (12)
wherein {circumflex over (x)}k|k−1 represents a state one-step prediction amount of at the k−1 moment, zk represents an observation vector at the k moment, Pk|k−1 represents a one-step prediction covariance, {circumflex over (z)}k|k−1 represents one-step measurement prediction, Pzz,k|k−1 represents a one-step measurement prediction variance, and Pzz,k|k−1 represents a cross covariance;
step 3.8:
updating the quaternion:
{circumflex over (x)}k=[δ{circumflex over (p)}kT{circumflex over (β)}kT]T Formula (13)
wherein {circumflex over (β)}kT represents the transposition of a gyro drift estimation value at the k moment, and δ(⋅) represents an error;
and step 3.9:
resetting δ{circumflex over (p)}k as a zero vector to prepare for a next filtering cycle.

444 more elements

Facts

Application

18/426,210Filed 29 Jan 2024

Priority

CN 202211423293.515 Nov 2022

Published

30 May 2024

Term ends

Not estimatedThe source records don’t support an estimate.

Classification8 codes

G01C 21/025
By astronomical means: with the use of startrackers
G01C 21/20
Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00: instruments for performing navigational calculations
B64G 1/244
Guiding or controlling apparatus, e.g. for attitude control: spacecraft control systems
B64G 1/245
Spacecraft control systems: attitude control algorithms for spacecraft attitude control