VBN Filter

4.2 VBN Filter

The VBN filter is based on the well-known EKF. Kalman filtering is a form of sequential state estimation based on weak stationary stochastic processes. An exhaustive discussion on this topic can be found in [291]. For full-order estimators such as the Luenberger observer, the poles have to be selected manually to obtain stability while satisfying signal filtering requirements. In Kalman filters, the poles are rigorously placed using the stochastic properties of measurement and model errors. Kalman filtering, in its linear or non-linear formulation, assumes non-correlated, zero-mean Gaussian white-noise processes.

4.2.1 Discrete-Time Kalman Filter

A discrete linear system, considering stochastic noises, takes the form:

xk+1= Fkxk+ Bkuk+ Υ_kwk (4.33a)

yk = Hkxk+vk (4.33b)

wk and vk are non-correlated zero-mean Gaussian white-noises:

E

wkw^Tj =

!0 k = j

Q^k k = j (4.34a)

E

vkv^Tj =

!0 k = j

Rk k = j (4.34b)

E

vkw^Tk = 0 ∀k (4.34c)

whereE {x} is the expected value of x. Q^k andR^k are discrete-time covariance matrices.

The measurement equation (4.33b) relates the observation vectoryk to the state vector xk .

From the continuous-time state-space matrices A and B, their discrete versions are given by:

Fk= e^AΔt (4.35a)

Bk=

"# _Δt

0 dτ e^Aτ

$

B (4.35b)

where Δt is the sampling time.

Chapter 4. Navigation & Docking System

The discrete time Kalman filter is presented in Table 4.1.

Table 4.1 – Discrete-time Kalman filter.

Model xk+1= Fkxk+ Bkuk+ Υ_kwk,wk∼ N (0, Qk) yk= Hkxk+vk,vk∼ N (0, Rk)

Initialise xˆ₀=x₀ P0 = E

x˜0x˜^T₀

Innovation Δyk=

y˜k− Hkxˆ⁻_k Sk=

HkPk⁻Hk^T+ Rk



Gain Kk= Pk⁻Hk^TSk⁻¹

Update xˆ⁺_k = ˆx⁻_k + K^kΔyk

Pk⁺= [1 − KkHk] Pk⁻

Propagate xˆ⁻_k+1= Fkxˆ⁺_k + Bkuk

P_k⁻₊₁ = FkP_k⁺Fk^T+ Υ_kQkΥ^T_k

In Table 4.1, ˜x ≡ ˆx − x and ˆx is the estimated variable, and ˜yk is the measured value of yk. The subscript + designates the a posteriori value and the− the a priori value.

P is the covariance matrix of the system. The square root of its diagonal represents the estimation error covariance of the state estimation error. However, in orbit, a reference value of the state is not available, and the estimation error cannot be computed to assess the filter performances.

The filter performance can be assessed using the values computed during the innovation step. The innovation error covariance, given by the square root of the innovation covariance Sk diagonal, can be compared to the measurement innovation error Δyk. If the error covariance does not correspond to the measurement innovation error, the filter sensor noise or dynamics process noise covariance matricesRkandQk, are probably badly tuned or that a sensor is defective [292].

The effect of the sensor noise covariance matrix is illustrated in Figures 4.14, 4.15, and 4.16.

4.2. VBN Filter

In this example, an EKF is fed with a star tracker measurement and estimates the attitude.

The attitude determination problem will be further discussed in Section 4.6. When the measurement noise covariance matrix is well tuned, the estimation and innovation errors are well bounded by their respective covariance errors. This is a good indication that the filter is well tuned.

(a) Estimation error and 1σ bound covariance error.

(b) Innovation error and 1σ bound innovation covariance.

Figure 4.14 – EKF with near optimal tuning parameters. The estimation and innovation errors are consistent with their respective covariance errors. The EKF is close to an optimal tuning.

Chapter 4. Navigation & Docking System

(a) Estimation error and 1σ bound covariance error.

(b) Innovation error and 1σ bound innovation covariance.

Figure 4.15 – EKF with an over-filtered measurement. The innovation error is much smaller than the innovation covariance meaning that the measurement covariance matrix R^k parameters are too large.

If the estimation and innovation measurement errors are not consistent with their respec-tive covariance errors, it indicates poorly tuned filters.

Consequently, although the estimation error cannot be computed in orbit, the Kalman filter innovation step contains information which can be used to assess its performances.

4.2. VBN Filter

(a) Estimation error and 1σ bound covariance error.

(b) Innovation error and 1σ bound innovation covariance.

Figure 4.16 – EKF with an under-filtered measurement. The innovation error is much larger than the innovation covariance meaning that the measurement covariance matrix R^k parameters are too small.

4.2.2 Discrete-Continuous Extended Kalman Filter

In most cases, the measurement equations and the dynamics are non-linear and continuous:

x = f(x, u) + Gw˙ (4.36a)

y = h(x) + v (4.36b)

However, the measurements provided by the sensors are mostly discrete. The dynamics

Chapter 4. Navigation & Docking System

filter can be used. Another solution is to use the EKF formulation, which takes benefit from the non-linear equations during the innovation and propagation steps, but using discrete measurements. However, for onboard implementation, it is preferable to perform the propagation steps with a discrete state-space as this requires less computational resources. Furthermore, the VBN is only used in close-range where non-linearities are small, as shown in Section 3.4.

The state-vector discrete propagation is given by (4.33a). For the covariance matrix propagation, special care must be given to the process noise covariance matrix Qk if it is originally defined using a continuous-time system. For the P2P dynamics discussed in Section 3.4 the environmental and sloshing perturbation errors are continuous. The continuous-time covariance matrix propagation is expressed as:

P = AP + P A˙ ^T+ GQG^T (4.37)

whereQ is the process noise matrix which defines w in (4.36a): w ∼ N (0, Q).

It can be shown that when the sampling time Δt is below Nyquist’s limit, the following is true [291, p. 172]:

Υ_kQkΥ^T_k = ΔtGQG^T (4.38)

Using the linearised P2P dynamics and the non-linear measurement equations (4.16), which are of the formy = h(x) and can be used with discrete or continuous variables, the Continuous-Discrete EKF is provided in Table 4.2.

Special care must be taken during the update step, as some of the components of the state vector are Euler angles and thus not additive. The update equation has the form:

z⁺=z⁻+ Δz (4.39)

For non-additive angles, it becomes:

R123(α⁺, β⁺, γ⁺) = R123(Δα, Δβ, Δγ)R123(α⁻, β⁻, γ⁻) (4.40)

The updated angles can then be obtained using the algorithm presented in Appendix B.1.

The process noise covariance matrix involved in the filtering has the following structure:

Q = diag

σ²_α σ_ω² σ²_s σ²_˙s 

(4.41)

4.2. VBN Filter

Table 4.2 – Continuous-Discrete Extended Kalman filter. ¯x, ¯u are the fixed linearisation points of the non-linear dynamics.

Model x = f(x, u) + Gw, w ∼ N (0, Q)˙ yk =h(xk) +vk,vk ∼ N (0, Rk)

Initialise xˆ₀ =x₀ P0= E

x˜0x˜^T₀

Innovation Δyk=

y˜k− h(ˆx⁻_k) S^k=

H^k(ˆx⁻_k)Pk⁻Hk^T(ˆx⁻_k) + R^k Hk(ˆx⁻_k) = ^∂_∂^h_x

ˆx⁻_k

Gain Kk= P_k⁻H_k^T(ˆx⁻_k)S_k⁻¹

Update xˆ⁺_k = ˆx⁻_k + KkΔyk

Pk⁺= [1 − KkHk] Pk⁻

Propagate xˆ⁻_k+1= F^kxˆ⁺_k + B^kuk

Pk⁻+1= FkPk⁺Fk^T+ ΔtGQG^T Fk= e^AΔt, withA = ^∂_∂^f_x

¯x,¯u

Bk =%_Δt

0 dτ e^Aτ

B, with B = _∂^∂f_u

¯x,¯u

The attitude angles and the position are uniquely determined by their respective dynamics.

Given an initial condition, their value can be obtained by respectively integrating the rotation rate and the velocity. Thusσ²_α= σ_s²= 0. The values for σ_ω² andσ²_˙s will need to be selected to achieve the required filter accuracy.

The danger with such filters lies in the fact that the Kalman gain must be recomputed at each sampling time, increasing the chances of divergence. This can be avoided by selecting steady-state Kalman filters in which asymptotic values are used to pre-compute a constant gain. This solves the divergence problem, but the filter will require more time to converge.

Chapter 4. Navigation & Docking System

For the VBN, this approach cannot be used, as the measurement equations (4.16) depend explicitly on the state-vector. Asymptotic values can thus not be obtained. However, as discussed, the analytical solution can be used to monitor the EKF performance and as a backup in case of failure.

Two different EKFs are needed:

1. The first filter will use the outer and central LEDs combined with the star trackers measurements that the chaser and target are providing. This filter is used in the 10 m to 5 m range.

2. The second filter will rely only on the 5 LEDs of the central cross pattern and will be used from 5 m until docking.

As using the ISL communication adds a potential point of failure, the VBN for the final 5 m will not include the star trackers measurements and only rely on the camera. This choice impacts the sizing of the central pattern.

4.2.3 Outer Pattern Measurement Equations

The observation vector yk of the outer pattern contains the LEDs image in the camera frame and is described by (4.16). The LEDs positions on this image are given by the second and third components of xⁱn_c,i = a, b, 5. The observation vector contains, in addition, the relative attitude angles provided by the star trackers (4.19). The vector yk

has the following form:

(4.42) must be linearised around the state vector’s current best estimate. The matrix H = ^∂_x^h has been computed using the MATLAB^® code provided in Appendix D.2.6.

The associated measurement noise covariance matrix has the following form:

Rk= diag

In document Guidance, Navigation and Control for Autonomous Rendezvous and Docking of Nano-Satellites (Page 125-133)