Tracking Kalman Filter Applied to a Satellite Control System Simulation Using Reaction Wheel Assembly Actuators Steven C. Rogers
Kalman filters are commonly used for state estimation. In this case the Kalman filter will be used for error state estimation. The error states will be used as input to the attitude control algorithm. The error state filter will be designed as a tracking filter instead of the traditional low pass non-tracking filter. A tracking filter takes advantage of additional error states for increased accuracy. The design will be applied to a generic satellite driven by reaction wheel assemblies (RWA). Four RWA’s are occasionally used for attitude control of satellites. In the case that one of the RWA’s becomes damaged the satellite attitude may still be controlled in all three axes with reasonable reliability. The RWA’s are usually installed with each RWA axis offset from the principal axes by a specified angle, which still enables a torque control about any one of the satellite principal axes. The incapacity of any of the RWA’s can be compensated by the remaining suite of torque capabilities. A possible geometrical configuration of a control system may be based on four RWA’s each inclined to the Xb-Yb principal axis by an angle β. Because of the inclination each RWA applies a torque in the Zb principal axis as well. The RWA torque vector has four elements which have a matrix relationship to the three principal satellite axes. If one of the RWA torque contributions is degraded the other RWA’s must make up for the loss in effectiveness. In this paper several multivariable control schemes will be developed and applied to a satellite simulation with a control system based on four RWA’s. The several control schemes will have a multivariable Kalman filter providing the error state estimates. A tracking Kalman filter will be compared to a nominal nontracking filter. Keywords: Adaptive Noise Control, ASE Filters, model-based adaptive filters, neural networks
Nomenclature FDI e s ν1 y ν0 p
= = = = = = =
fault detection and isolation error signal of interest disturbance corrupted measurement signal adaptive filter output noise source signal (strain gage equivalent) roll rate
1
q r d a b Îź c
= = = = = = =
pitch rate yaw rate primary sensor signal filter coefficients of input filter coefficients of output adaptive gain for filter adaptive exogenous coefficients
I. Introduction ALMAN filters are commonly used for state estimation in satellite attitude control applications. The Kalman filter requires that all error states be modelable as zero mean noise processes with known variances, power spectral densities, and time correlation parameters. In this study the Kalman filter will be used for error state estimation. The error states will be used as input to the attitude control algorithm. Kalman filter estimation requires that the signal generating process be described by a linear or linearized mathematical model. The error state filter will be designed as a tracking filter instead of the traditional low pass non-tracking filter. A tracking filter takes advantage of additional error states for increased accuracy.
K
The Kalman filter ideas may also be combined with passive fault tolerant control laws. The analytical fault-tolerant operation can be achieved either passively by the use of a control law designed to be insensitive to some known faults, or actively by an FDI (fault detection and isolation) mechanism, and the redesign of a new control law. The active methods are less realistic because all the faults that may affect the system cannot be known a priori. Figure 1. Typical Fault-Tolerant Control This paper shall use multivariable passive fault-tolerant control System. algorithms, that is, there is no redesign of the control law or a fault detection system. Inputs to the control laws shall come from Kalman filter designs. Passive algorithms include robust systems, adaptive components augmenting traditional SISO (single input single output) control algorithms, and multivariable robust control systems. Robust control designs, including H-infinity are designed to accommodate model uncertainties and structured disturbances. A model of a plant can never be perfect, and as such will always be an approximation of the true plant. Occasionally some characteristics of a plant will remain unmodelled. This occurs because their Figure 2. Actuator, Plant, and Sensor Faults. contribution is considered small, they are not well known, they may be difficult to model, or they change with varying flight conditions. A controller is said to exhibit good robust stability if it remains stable for all variations in plant behavior which are expected to occur. Example development and simulations of each category will be included in the paper. The control laws shown in this paper are multivariable pole placement, linear quadratic regulator, and H-infinity. Kalman filter tracking and non-tracking formulations are compared. Also, a means of monitoring the covariance matrix for stability is shown that allows covariance matrix resets based on stability.
II. Control Laws Closed-loop pole locations have a direct impact on time response characteristics such as rise time, settling time, and transient oscillations. Root locus uses compensator gains to move closed-loop poles to achieve design specifications for SISO systems. You can, however, use state-space techniques to assign closed-loop poles. This design technique is known as pole placement, which differs from root locus in the following ways:
2
• •
Using pole placement techniques, you can design dynamic compensators. Pole placement techniques are applicable to MIMO systems.
Pole placement requires a state-space model of the system. In continuous time, such models are of the form x = Ax + Bu , y = Cx + Du , where u is the vector of control inputs, x is the state vector, and y is the vector of measurements. Under state feedback u = −Kx , the closed-loop dynamics are given by x = ( A − BK ) x and the closed-loop poles are the eigenvalues of A-BK. A gain matrix K can be computed that assigns these poles to any desired locations in the complex plane (provided that (A, B) is controllable). The state-feedback law can’t be implemented unless the full state x is measured. However, you can construct a state estimate such that the law retains similar pole assignment and closed-loop properties. You can achieve this by designing a state estimator (or observer) of the form x = Ax + Bu + L( y − Cx − Du ) . The estimator poles are the eigenvalues of A-LC, which can be arbitrarily assigned by proper selection of the estimator gain matrix L, provided that (C, A) is observable. Generally, the estimator dynamics should be significantly faster than the controller dynamics (eigenvalues of A-BK). The Linear Quadratic Regulator (lqr) is a commonly used type of state space control algorithm. It is frequently used for comparison purposes as it is a very useful means to obtain a state estimator as well as a state feedback control. It does require a linearized dynamic model of the system to be controlled, or a nonlinear model that is convenient to linearize. For a linear model expressed as: x = Ax + Bu , x ∈ℜn (1) We can propose a performance index: ∞ 1 J = ∫ x T Qx + u T Rudt (2) 20 In these equations x is the bounded state vector, u is the control input vector, A is the control matrix, B is the input matrix, Q is the state performance matrix, and R is the input performance matrix. There are no terminal constraints on the control system. The above may be solved by use of the maximum principle as below.
H = x T Qx + u T Ru + λT ( Ax + Bu ) T
∂H x = = Ax + Bu ∂λ T
∂H T − λ = = Qx + A λ ∂λ ∂H 0= = Ru + λT B ⇒ u = −R −1 B T λ ∂u By using the terms of eqn 3 we may arrive at the classical lqr solution as:
(3)
λ = Px ⇒λ = P x + Px
λ = P x + P ( Ax − BR −1 B T P ) x = −Qx + AT Px − P = PA + AT P − PBR −1 B T P + Q (4)
u = −R −1 BPx
is set to 0, then we have The 3rd line in the equation above is called the Riccati ordinary differential equation. If P the algebraic Riccati equation which may be solved by numerous software packages. With minor modifications the same approach can produce a state estimator. A direct H∞ solution is given by minimizing γ in the equation below.
3
γ = inf K
K −1 ~ −1 ( ) I I −GK M
∞
(5) Note that in this approach the classical γ -iteration is avoided. Given a minimal realization [A, B, C, 0] of a controllable and observable plant the control algebraic riccati equation (CARE) and filtering algebraic riccati equation (FARE) are given. A* X + XA − XBB * X + C * C = 0 AZ + ZA* − ZC *CZ + BB * = 0
The optimal γ is given by:
(6)
γ min = (1 + λmax ( XZ ) ) 1 2
(7)
The controller is then:
A + HC + γ 2 BB * XW * − 1 − H 1 K= 2 * * −1 γ B XW1 0 W1 = I + XZ − γ 2 I
(8)
H = − ZC * III. Kalman Filters Kalman filters are commonly used for guidance and navigation systems3,4 due to their optimal estimation capabilities. The two main features of the Kalman formulation and solution are 1) vector modeling of the random processes under consideration and 2) recursive processing of the noisy measurement (input) data. The equations for the discrete form of the Kalman filter are shown in Figure 3. As is shown the Kalman filter provides a linear iterative solution to a weighted least-squares problem between and at the times of update. In general 3,4 the filter may begin at any point between the four boxes; Figure 3. Kalman Filter Loop however, it usually begins before the project ahead box. Good explanations of the Kalman filter are included in many standard texts; consequently, they will not be repeated here. We will confine ourselves to explaining the difference between tracking and non tracking in the case of position, velocity, and acceleration. The famous alphabeta and alpha-beta-gamma filters5 are well-known examples of tracking filters that are found in many textbooks. The key differentiator in such tracking filters is that the position output relies on velocity and acceleration estimates as well as on its own estimate. In a Kalman filter the Hk matrix is adjusted accordingly, while the other components remain the same. The following matlab code snippet illustrates the difference. if strcmp(trackon,'track') H = [1 dt dt*dt/2]; else H = [1 0 0]; end
4
As shown above when the tracking feature is engaged all states contribute to the position output, whereas when tracking is not on only the position state contributes to the position output. Note that the above H matrices are used as building blocks to construct the roll-pitch-yaw multivariable output. The bottom block of Figure 3 that has the equation for computing the error covariance update presents a good opportunity for Kalman filter stability monitoring. The term ( I − K H ) determines the overall Kalman filter stability,
1
KF covariance coef poles
0.95
0.9
0.85
0.8
0.75
k
0.7
0.65 0
50
100
150 time (sec)
200
250
300
k
in particular, the stability of the Kalman filter error covariance matrix. The eigenvalues of the above term must lie within the unit circle for overall stability.
Figure 4. Kalman Filter Error Covariance Matrix diagonal Values Figure 4 shows values of the diagonal of the coefficient matrix for a typical simulation run. The values tend to remain near 1.0. They drop slightly when set point changes occur. This appears to be a potential indicator of when to reset the covariance matrix. It would also appear that the reset value could be determined according to stability, that is, when the poles increase beyond 1.0 determine a reset point by point along the diagonal. This is a computation that will vary according to the Hk structure. If the calculation is performed diagonal point by point the applicable diagonal values of the covariance matrix Pk can be computed.
IV. Simulation The simulation is based on a generic satellite model. The satellite rigid body dynamics is modeled as a simple set
− w3 0 w1
0 = −ΩJw + M , Ω = w3 of nonlinear equations Jw − w2
w2 − w1 , where w is the angular rate vector [roll, 0
pitch, yaw], J is the mass moment of inertia matrix, and M is the moment vector. For this application the matrix 0 1269 −106 0 moment of inertia J = −106 1272 . The moment vector is given in the three axes. Note that for 0 1524 0 redundancy most satellites are designed with four reaction wheel assemblies (RWA’s). Each is placed on an axis, but tilted at an angle β in order to influence more than one axis. Following the example in ref 5 we have the equations:
T T1 Tˆcx Tcx / cβ 1 0 − 1 0 1 T T2 ˆ 2 Tcy = Tcy / cβ = 0 1 0 − 1 T = [ Aw ] T = [ Aw ]T 3 Tˆ T / sβ 1 1 1 1 3 T cz cz 4 T4 (9) In the above cβ = cos(β), sβ = sin(β), and β is the RWA inclination angle to the appropriate axis. The matrix Aw in equation 13 is not square and is not easily inverted, therefore another equation is needed. Since the summation of moments is zero, we can use the following 0 = T1 − T2 + T3 − T4 . We can now rewrite equation 13 as:
5
Tˆcx 1 ˆ Tcy = 0 Tˆcz 1 0 1
0
−1
1 1 −1
0 1 1
0 T1 − 1 T2 1 T3 − 1 T4
(10)
0 T1 1 T 0 1 2= 1 T3 2 − 1 0 T4 0 −1
0.5 0.5 0.5 0.5
0.5 Tˆcx − 0.5Tˆcy 0.5 Tˆcz − 0.5 0
(11) With the above we have the transformation between the three axes command control torques and the four wheels’ command control torques. To achieve a zero-reaction failure for a single RWA a column of the matrix in equation 14 may be zeroed out. The above sequence of equations was implemented in matlab and simulink.
6
V. Results The different three different control strategies with different Kalman filters as explained above were tested on the above satellite model. The plots are shown below. The first comparison (Figures 7-12) is with 100% poleplace MV, Filter is type track RPY 1.01 0.94 x 10error = 1.56 0.1 2 RWA 1 failure. 0
0
0.2 0
100
200
300
200
-3
100
200
-3 RPY x 10error = 5.72 5
poleplace MV, Filter is type nontrack 0.1
300
300
0 -0.1
0
0.93
10.05
0 0
100
200
-5
300
0
100
200
300
0
100
200
300
0
100 200 time (sec)
300
-3
0.2 -0.2
5
0
100
200
x 10
-2
300
0.1 0
100
200
300
-5
0
100
200
0.2
300
-3
x 10 2 100
0
0
200
300
0 -0.2
0.01
-0.1 0
0
2
x 10
0 0
100
200
-2
300
0
0.2
-0.2
-3
0
-0.2
yaw (rad)
10.88
x 10
2 0
0
0
yaw (rad)
-5
1.19
100
pitch (rad)
pitch (rad)
pitch (rad)
0 -0.1
-3 RPY -2 = 6.00 x 10error 3005 0
roll (rad)
0 poleplace MV, Filter is type track -0.1 100 200 0.1 0
0
100 200 time (sec) 100 200 time (sec)
-2
3000 300
-0.01
0.2
0
0
100 200 time (sec)
100 200 time (sec)
yaw (rad)
roll (rad)
roll (rad)
-3
300
300
Figure 13. Multivariable Pole Placement with Tracking Filter Pole Placement & Tracking Figure 7. MIMO Filter with 100% RWA 1 Failure
0.01
0
0
-0.2
-0.01
0
100 200 time (sec)
300
Figure 8. MIMO Pole Placement & tracking filter with 100% RWA 1 Failure
non
A -3 RPY x 10error = 5.01 5
Hinf MV, Filter is type nontrack 0.1
roll (rad)
0.1 0 -0.1
0.90
15.28
roll (rad)
-3 RPY x 10error = 5.16 5
Hinf MV, Filter is type track
0 0
100
200
300
-5
0
100
200
0
0
-0.2
0
100
200
300
-2
x 10
0
100
200
0
100
200
300
0.2
2
0
0
-0.2
-2
0
0
-0.2
0
100 200 time (sec)
300
-0.02
yaw (rad)
yaw (rad)
0.02
0
100 200 time (sec)
0
100
200
300
RPY error = 12.71 0.01
roll (rad)
0
0
-0.1
-0.01
0
100
200
300
3.82
0
100
0
-0.2
-0.02
0
100 200 time (sec)
300
lqr MV, Filter is type nontrack
22.96 0.1
200
300
0
100
200
300
0
100 200 time (sec)
300
x 10
Figure 10. MIMO H-infinity & non tracking Filter with 100% RWA 1 Failure
roll (rad)
lqr MV, Filter is type track
200
0.02
0
300
Figure 9. MIMO H-infinity & Tracking Filter with 100% RWA 1 Failure 0.1
100
300
0.2 0.2
0
14.47
-3
pitch (rad)
pitch (rad)
2
0 -5
300
-3
0.2
0 -0.1
0.71
RPY error = 12.78 0.01
0
0
-0.1
-0.01
300
0
100
200
300
3.58
23.12
0
100
200
300
0
100
200
300
0
100 200 time (sec)
300
0 -0.2
x 10
pitch (rad)
5 0 0
100
200
300
-5
0.2
0.02
0
0
-0.2
-0.02
0
100 200 time (sec)
300
0
100
200
0.2
0.01
0
0
-0.2
300
0
100
200
300
0.2 yaw (rad)
yaw (rad)
pitch (rad)
-3
0.2
0
100 200 time (sec)
300
-0.01
0.02
0
0
-0.2
-0.02
0
100 200 time (sec)
300
Figure 12. MIMO LQR & non tracking Filter with 100% RWA 1 Failure
Figure 11. MIMO LQR & Tracking Filter with 100% RWA 1 Failure
7
300 second trajectory was simulated for each of the seven strategies. Across the title of each plot is listed the control method, whether the Kalman filter is tracking or non tracking, and the roll-pitch-yaw error vector. -3 RPY x 10error = 1.58 2
roll (rad)
poleplace MV, Filter is type nontrack 0.1 0 -0.1
0
100
200
300
-2
roll (rad)
roll (rad) pitch (rad)
pitch (rad)
-3
0.2
2
0
0
-0.2
-2
0
100
200
300
2
0
0
-0.1
-2
0
100 200 time (sec)
300
6.00 1.19 5.72 0.93 Hinf MV, Filter is type nontrack 1.44 0.98 0.1 1.41 0.78 0 5.16 0.9 -0.1 0 100 200 5.01 0.71 300 3.82 4.21 0.2 3.66 3.48 0 12.71 3.82 -0.2 0 100 200 12.78 3.58 300
0
100 200 time (sec)
Figure 15.
300
100 200 time (sec)
-3 RPY x 10error = 3.82 5
lqr MV, Filter is type track roll (rad)
0
0
0
-0.1
-5
100
200
300
0
100
200
300
100 200 time (sec)
300
x 10
0 x 10
0
-3
0.78
1.63
200
300
200
300
100 200 time (sec)
300
-3
0.2
4.21
100
5
0
0
-0.2
-5
300
MIMO H-infinity with Tracking Filter
0.1
0
x 10
yaw (rad)
0 -5
300
-3
0
100 200 time (sec)
Figure 16. Filter
3.46
300
200
lqr MV, Filter is type nontrack
300
0 -0.1
x 10
0
MIMO H-infinity with non tracking
0.1 roll (rad)
yaw (rad)
0 -0.2
200
10.88 10.05 RPY x 10error = 1.41 1.84 2 1.63 0 15.28 -2 0 100 14.47 x 10 3.46 2 3.04 0 22.96 -2 0 100 23.12
-3
5
100
Roll Error Pitch Error Yaw Error Figure 14. Multivariable Pole Placement with non 1.56 1.01 0.94 tracking Filter 1.58 1.02 0.98
-3
0.2
0
-3
yaw (rad)
Error in rad/sec MIMO pole placement, no Failure, tracking MIMO pole placement, no Failure, non tracking MIMO pole placement, RWA 1 Failure, tracking MIMO pole placement, RWA 1 Failure, non tracking Hinf MV, Filter is type track RPY 0.98 1.84 x 10error = 1.44 MIMO H-Infinity, no Failure, tracking 0.1 2 MIMO H-Infinity, no Failure, non tracking 0 0 MIMO H-Infinity, RWA 1 Failure, -2tracking -0.1 0 100 200 300 0 100 200 300 MIMO H-Infinity, RWA 1 Failure, non tracking MIMO LQR, no Failure, tracking 2 x 10 0.2 MIMO LQR, no Failure, non tracking 0 0 MIMO LQR, RWA 1 Failure, tracking -0.2 -2 0 RWA 100 1 Failure, 200 300 0 100 200 300 MIMO LQR, non tracking
0.98
0
0.1
Summary Table of Tracking Error Performance Characteristics
1.02
-3
pitch (rad)
According to the summary table below, multivariable control with the non tracking Kalman filter generally appears superior for the test cases evaluated. RWA 1 was 100% disengaged for the simulation comparison. Most likely, the control improvement noted with the non tracking Kalman filter is due to the smoother signal being generated. Velocity and acceleration states are more susceptible to noise corruption. Other reasons besides control would have to drive the use of a tracking control in a control loop based on this study alone.
-3 RPY x 10error = 3.66 5
3.48
3.04
0 0
100
200
300
-5
0
100
200
300
100
200
300
100 200 time (sec)
300
0.2
0.01
0
0
-0.2
0
100
200
300
-0.01
pitch (rad)
pitch (rad)
-3
0
100
200
300
0.2
5
0
0
-0.2
-5
0
100
200
300
x 10
0
0.2
5
0
0
-0.2
-5
0
Figure 17.
100 200 time (sec)
300
-3
0.2 yaw (rad)
yaw (rad)
-3
x 10
0
100 200 time (sec)
300
MIMO LQR with Tracking Filter
5
0
0
-0.2
-5
0
Figure 18.
8
100 200 time (sec)
300
x 10
0
Pole Placement With Smoothed Q
VI. Conclusion Control law principles were discussed in the context of failure of a satellite reaction wheel control assembly. Three different combinations of multivariable control laws provided the testbed for comparing Kalman filter implementations for state estimation in the control of satellite attitude using four reaction wheel assemblies (RWA). The control law/state estimation schemes were applied to all three roll-pitch-yaw channels of the satellite attitude control system. In most cases the control performance was improved by the non tracking state estimation. A covariance matrix reset based on stability was investigated in a preliminary way. Future work should include additional aerospace model applications, addressing the stability issues for adaptive components, additional neural network structures, and testing on more realistic maneuvers (sensor calibration, orbital transitions, and transitioning to different sensors).
References 1
Glover, K & McFarlane, DC, ‘Robust Stabilization of Normalized Co prime Factor Plant Descriptions with H∞-Bounded Uncertainty.’ IEEE Transaction on Automatic Control, 34(8):821-830, Aug., 1989. 2 McFarlane, DC, ‘Robust Controller Design Using Normalized Co prime Factor Plant Descriptions,’ PhD thesis, University of Cambridge, 1988. 3 Kayton, M., Fried, W., Avionics Navigation Systems, 2nd Edition, Wiley-Interscience, 1997. 4 Brown, R., Hwang, P., Introduction to Random Signals and Applied Kalman Filtering, 3rd Edition, John Wiley & Sons, 1997 5 Lin, C., Modern Navigation, Guidance, and Control Processing, Prentice Hall, New Jersey, 1991. 6 Sidi, M. J., Spacecraft Dynamics and Control, Cambridge University Press, Cambridge, 1997.
Appendix The accompanying matlab code was used for the Kalman filter comparison. alph = 0.99; niter = 2; dt = t - X.tim; F = [1 dt dt*dt/2;0 1 dt;0 0 1]; F = [F,zeros(3,6);zeros(3,3),F,zeros(3,3);zeros(3,6),F]; % if trackon == 'track' if strcmp(trackon,'track') H = [1 dt dt*dt/2]; H = [H,zeros(1,6);zeros(1,3),H,zeros(1,3);zeros(1,6),H]; else H = [1 0 0]; H = [H,zeros(1,6);zeros(1,3),H,zeros(1,3);zeros(1,6),H]; end for i = 1:niter err = data(:) - H*X.x; Qtemp = abs(err);%Qtemp = Qtemp.*Qtemp; Q = diag([Qtemp(1) 0 0 Qtemp(2) 0 0 Qtemp(3) 0 0]); X.P = alph*F*X.P*F' + Q; % if 0 ~= (data - X.x(1)) % X.R = alph*X.R + (1-alph)*(data - X.x(1))^2; % X.R = alph*X.R + (1-alph)*diag(X.error.*X.error); X.R = alph*X.R + diag(err.*err); % end Kgain = X.P*H'/(H*X.P*H'+X.R); X.x = F*X.x; X.x = X.x + Kgain*err;
9
temp = eye(size(X.P)) - Kgain*H; X.P = temp*X.P*temp' + Kgain*X.R*Kgain'; end out = [H*X.x;diag(temp,0)];
10