Bias corrected inertial navigation system
View Patent ↗A method and system is provided for estimating and compensating for gyro bias in gyro-stabilized systems. The method includes comparing an output of a gyroscope to a reference measurement; estimating a bias of the gyroscope based on the comparison using a Kalman filter; and adjusting a control output of the gyro-stabilized system with the estimated bias to maintain a position of the gyro-stabilized system.
1. A method for controlling a gyro-stabilized platform including an inertial navigation system with an inertial measurement unit, said method comprising the steps of:
measuring changes in an angular rate of the platform with the inertial measurement unit;
determining an orientation of the reference system with the inertial measurement unit;
outputting at least one signal value based on said determining the orientation step;
filtering the at least one signal value through a low pass filter by estimating a bias based on a comparison using a Kalman filter as the low pass filter wherein parameters are based on an Allan Variance statistical model as a time domain analysis to characterize underlying noise processes such that a Rate Random Walk noise component and an Angle Random Walk noise component are separated for obtaining a contribution of each noise component by:
calculating the Allan Variance statistical model for a set of data from
a
(
τ
)
=
1
2
(
N
-
1
)
∑
k
=
1
N
-
1
(
y
_
k
+
1
-
y
_
k
)
2
where alpha is a square root of an Allen two-sample variance, N is the number of frequency
expressing a dynamic system model as
[
θ
.
B
.
]
=
[
0
-
1
0
0
]
[
θ
B
]
+
[
1
0
]
ω
+
[
η
v
η
u
]
;
where the variables are angular attitude, θ and bias, B, the Angle Random Walk noise component, η v , is on the order of R, the Rate Random Walk noise component, η u , is on the order of Q;
converting the dynamic system model to discrete-time by the forms
x k+1 =Φ k x k +Γ k u k +Y k w k
{tilde over (y)} k =H k x k +v k ;
expressing the discrete-time dynamic system model by
[
θ
B
]
k
+
1
=
[
1
-
dt
0
1
]
[
θ
B
]
k
+
[
1
0
]
ω
k
θ
_
k
=
[
1
0
]
[
θ
B
]
k
+
v
k
;
initializing an estimate matrix by
x
^
0
=
[
θ
~
0
0
]
where {tilde over (θ)} is the initial angular attitude measurement;
initializing a covariance matrix “p” by
P
=
[
poa
0
0
pog
]
where “poa” is a measure of the initial accuracy of an angular attitude estimate, and “pog” is a measure of the initial accuracy of a bias estimate;
calculating a process noise covariance matrix Q as
Q
=
[
σ
v
2
dt
+
1
3
σ
u
2
dt
3
-
1
2
σ
u
2
dt
2
-
1
2
σ
u
2
dt
2
σ
u
2
dt
]
where σ v 2 and σ u 2 are the Angle Random Walk and Rate Random Walk variances, R and Q respectively;
expressing a measurement noise covariance matrix, R, as
R=σ n 2
where σ n 2 is the variance of the measurement noise;
calculating a gain matrix K using a covariance matrix P and a measurement noise matrix R, as
K k =P k H k T [ H k P k H k T +R k ] −1 ;
updating the covariance matrix P and an estimate matrix {circumflex over (x)} based on calculated gain and a value of the measured variable by
P k + =[1− K k H k ] P k −
{circumflex over (x)} k + ={circumflex over (x)} k − +K k [ {tilde over (y)} k −H k x k − ];
propagating the estimate matrix {circumflex over (x)} and the covariance matrix P in accordance with a discrete-time dynamic model as
{circumflex over (x)} k+1 − =Φ k {circumflex over (x)} k + +Γ k u k
P k+1 − =Φ k P k + Φ k T +Y k Q k Y k T ;
expanding for three-dimensional operation, resulting in the Kalman filter with six variables, expressed as
{circumflex over (x)} =[ϕθψ B x B y B z ] T
where Ø is roll, θ is pitch, φ is yaw, and B x , B y , and B z are gyro bias estimates;
differentiating an Euler Angle rotation transformation matrix for an Euler Angle Rate transform matrix, E, expressed as
E
=
[
1
sin
(
ϕ
)
tan
(
θ
)
cos
(
ϕ
)
tan
(
θ
)
0
cos
(
ϕ
)
-
sin
(
ϕ
)
0
sin
(
ϕ
)
/
cos
(
θ
)
cos
(
ϕ
)
/
cos
(
θ
)
]
;
calculating global rates of roll, pitch, and yaw from
[
ϕ
.
θ
.
ψ
.
]
=
[
1
sin
(
ϕ
)
tan
(
θ
)
cos
(
ϕ
)
tan
(
θ
)
0
cos
(
ϕ
)
-
sin
(
ϕ
)
0
sin
(
ϕ
)
/
cos
(
θ
)
cos
(
ϕ
)
/
cos
(
θ
)
]
[
ω
x
+
B
x
+
η
vx
ω
y
+
B
y
+
η
vy
ω
z
+
B
z
+
η
vz
]
;
determining gyro bias states from η u , as
[
B
.
x
B
.
y
B
.
z
]
=
[
η
ux
η
uy
η
uz
]
;
combining the dynamic model as
[
ϕ
.
θ
.
ψ
.
B
.
x
B
.
y
B
.
z
]
=
[
O
E
O
O
]
[
ϕ
θ
ψ
B
x
B
y
B
z
]
+
[
E
O
O
O
]
[
ω
x
ω
y
ω
z
0
0
0
]
+
[
E
O
O
I
]
[
η
vx
η
vy
η
vz
η
ux
η
uy
η
uz
]
where ‘O’ represents a zero matrix, and ‘I’ represents an identity matrix;
converting the dynamic system model into a discrete-time form to yield
[
ϕ
θ
ψ
B
x
B
y
B
z
]
k
+
1
=
[
I
Edt
O
I
]
[
ϕ
θ
ψ
B
x
B
y
B
z
]
k
+
[
Edt
O
O
Idt
]
[
ω
x
ω
y
ω
z
0
0
0
]
k
+
[
Edt
O
O
Idt
]
[
η
vx
η
vy
η
vz
η
ux
η
uy
η
uz
]
defining initialization of the covariance matrix as
P
=
poa
0
0
0
0
0
0
poa
0
0
0
0
0
0
poa
0
0
0
0
0
0
pog
0
0
0
0
0
0
pog
0
0
0
0
0
0
pog
;
defining a process noise matrix as
Q
=
A
0
0
C
0
0
0
A
0
0
C
0
0
0
A
0
0
C
C
0
0
B
0
0
0
C
0
0
B
0
0
0
C
0
0
B
where
A
=
σ
v
2
dt
+
1
3
σ
u
2
dt
3
,
B
=
σ
u
2
dt
,
and
C
=
-
1
2
σ
u
2
dt
2
;
defining the measurement noise matrix as
R
=
[
σ
n
2
0
0
0
σ
n
2
0
0
0
σ
n
2
]
;
and
generating estimates of inherent bias of the first fiber optic gyroscope, second fiber optic gyroscope and third fiber optic gyroscope;
adjusting the at least one signal value in accordance with a disturbance model subsequent to and in conjunction with said filtering the at least one signal value through a low pass filter step;
inputting the at least one signal value to three servo drive and torque motor assembles to adjust an angular rotation of the platform; and
maintaining a position of the gyro-stabilized platform based on said inputting and adjusting step.