acts as a middle ground between the other two alternatives. Furthermore, using a lph.D. Student, Aerospace Engineering, Texas A&M University, TAMU 3141.
The Journal of the Astronautical Sciences, Vol. 58, No.3, July-September 2011, pp. 461-478
Considering Measurement Model Parameter Errors in Static and Dynamic Systems Drew P. Woodbury/ Manoranjan Majji,2 and John L. Junkins3
Abstract
In static systems, state values are estimated using traditional least squares techniques based on a redundant set of measurements. Inaccuracies in measurement model parameter estimates can lead to significant errors in the state estimates. This paper describes a technique that considers these parameters in a modified least squares framework. It is also shown that this framework leads to the minimum variance solution. Both batch and sequential (recursive) least squares methods are described. One static system and one dynamic system are used as examples to show the benefits of the consider least squares methodology.
Introduction In static systems, state values are estimated using traditional least squares techniques based on a redundant set of measurements. Often these measurement models include additional parameters, whose errors affect the accuracy of the state estimates. Usually these parameters are known within certain accuracy limits based on manufacturing specifications or from calibration procedures. In complex measurement scenarios, however, these additional parameters may not be known as precisely as desired. Three possible methodologies exist on how to handle these errors: (1) assume the current parameter estimates are adequate and ignore the additional errors during state estimation; (2) augment the state vector to include the parameters as additional states; and (3) consider the parameters by introducing additional covariance terms into the estimator. Ignoring the parameter errors can lead to large bias errors in the estimated states, whereas augmenting the state vector can be cumbersome and computationally intensive for complex systems, "considering" the parameters acts as a middle ground between the other two alternatives. Furthermore, using a lph.D. Student, Aerospace Engineering, Texas A&M University, TAMU 3141. 2Postdoctora1 Assistant, Aerospace Engineering, Texas A&M University, TAMU 3141. 3Distinguished Professor, Aerospace Engineering, Texas A&M University, TAMU 3141.
461
462
Woodbury, Majji, and Junkins
consider approach is very beneficial for parameters that have low observability. As an example, estimating the gravity field of an asteroid is difficult from a single pass or trajectory [1]. Gravity coefficients are available from previous flybys, but there may not be enough data to improve these estimates from a single pass using a fully augmented state vector. By using a consider approach the problem is simplified while still accounting for the errors in the estimates. The data collected can then be used to improve the estimated gravity coefficients offline using more sophisticated methods. Consider filtering was first developed by S.P. Schmidt in the late 1960s [2]. Schmidt's formulation was designed to account for parameter uncertainty in dynamic and measurement models used in the traditional Kalman filter. His results are based on a minimum variance approach, but he does not go into the details of how to derive the result. A few years later, Jazwinski published his book where he provides a derviation of the consider (i.e., Schmidt-Kalman) Kalman filter [3]. Just as important, however, was that he showed how to apply the consider filter to nonlinear systems via the same approach used to derive the Extended Kalman Filter (EKF). More recently, Tapley, Schutz, and Born, describe consider analysis from both a batch and a sequential perspective [5]. It should be noted that consider analysis is different than consider filtering. Consider analysis provides an additional update to the traditional Kalman filter to account for parameter uncertainty, while consider filtering actually uses the consider states in the filter itself [4]. Based on the previous work described above [2-5], this article presents the mathematical foundation of the consider approach while providing a consistent nomenclature framework with other current literature [6]. The consider methodology will be explored first from a batch estimation perspective using both least squares and minimum variance derivations. Different cases will be described based on the a priori information available. Next, the consider analysis will be expanded into a sequential or recursive formulation. Finally, two simple examples will be presented, one static and one dynamic. Focusing on the batch estimators these examples will show the advantages and limitations of using the consider approach for state estimation.
Batch Estimation A discrete linear measurement model with additional parameters is described as (1)
where y are the collected measurements, x are the states to be estimated, p are the parameter values, Hx and Hp are the measurement sensitivity matrices, k is the measurement step, and v is the measurement noise. Often parameters appear nonlinearly with the states, but following traditional linearization techniques equation (1) can still be used. For batch estimation, a set of m measurements can be collected as
(2)
Measurement Model Parameter Errors
463
(3)
where E { ... } is the expectation function and the individual measurement covariances, Rk , are defined by E{ v;} = 0,
(4)
E{ vjvJ} = Rl>ij
where 8ij is the Kronecker delta function and Rk
= Rjj.
Least Squares If the system is static, Xk + 1 = Xb then one possible method to account for the parameters in the batch state estimate is to create an adjusted measurement vector (5)
Minimizing the least squares cost function J=
1
-(y - yl W(y - y) 2
(6)
where W is a weighting matrix and y = Hxx, results in the traditional weighted least squares solution [6] (7)
If the true values of the parameters are used and the weight matrix is defined as W = R- 1, the covariance of is the familiar result
x
(8)
If, however, the true values of the parameters are not known and only approximate values are used then the covariance of the states is no longer the result shown in equation (8) and additional calculations are needed to properly estimate the state covariance. One alternative solution is to estimate the parameters using an augmented state vector (9)
z = [:] This redefines the true measurement model as
(10)
where Hz = [Hx Hp]. Replacing the new measurement equation for
y = Hi. in equation
y and letting
(6) produces the augmented least squares solution (11)
464
Woodbury, Majji, and Junkins
The covariance for the augmented state vector is now easily proven to be
_
~
~
T_ {[(X-X)][(X-X)]T}_[pxx Pxp]_ T - Ppx Ppp -(Hz WHJ
Pz-E{(z-z)(z~z)} -E ep_p) (P_p)
-I
(12)
where the submatnces are (13) (14) (15) (16)
Solving for only the state estimates, X, results in the consider least squares estimate
x = (PxxH~ + px~J)WY
(17)
The result in equation (17) can be thought of as the weighted sum of the measurements with respect to the state and the parameter estimates where the weights are defined by the covariance matrices Pxx and Pxp. Minimum Variance with A Priori Estimates
Minimum variance techniques provide the optimal estimate of the states based on probability. For consider analysis, the augmented state vector, z, is used to find the optimal minimum variance solution and then the estimates for the states, X, are extracted from the solution. The first derivation will explore the case where a priori estimates of both the states and the parameters are available. Special cases will then be shown where only state or parameter estimates are available a priori, as well as, the special case when no initial estimates are available. Finally, the case where the parameters are known exactly will be investigated as a verification of the previous developments. The minimum variance derivation is based on the assumption that the best estimate of can be found from a linear combination of the measurements and its a priori estimates
x
(18)
where M and N are matrices to be determined. In the remainder of the text, a + will be used to denote the a posteriori estimates while a-will be used to denote the a priori estimates. In the case where there are additional parameters in the measurement model as in equation (1) and the true value of the parameters are known, then y can be replaced by y as before. The solution is then known to be [6]
x+ = =
(H~R-I
Hx
+ p:x-I)-I (H~R-Iy + p:x-'x-)
1Hx + P--I)-l 1Y (HTR(HTR- + P--'~x xx x xx X - HTR-1H x pp )
(19)
Once again, however, the true values of the parameters are not typically known and only estimates are available. As a result, the augmented state vector, z is used instead to produce
Measurement Model Parameter Errors
465
(20) where P;' is the covariance of the a priori estimates, Z- . Additionally, the updated covariance, p/ ' is known to be [6] p/ = E{(z+ - z)(z+ - Z)T} = (H;'R- 1Hz + p;.-I)-I
(21)
Let p;. -I be defined by _-I
Pz
~
-
[Mxx Mxp] Mpx Mpp
(22)
then p;.-Ip;. =
[Z: Z:][~t
~:] = [~ ~]
(23)
where P;' is defined by equation (12). Solving for the submatrices of p;.-l yields Mxx = p_-I - p-1r)-1 xx + p_-lp_ xx xp (Ppp _ p-px'P--Ip-)-I xx xp p-px'p_-l xx = (P-xx - p xl" pp px
(24)
(25)
M p- p_-l MT - P- - Ip-)-I p- p_-I Mpx=- (Ppp- Ppx' xx xp px'xx = - Pl"px' xx = xp
(26)
Mpp = p;,1 + p;plp; (P;" - p;~;plp;x)-I P;~;pl = (Ppp - p;~;,,-Ip;,) - I
(27)
Expanding out the matrices in equation (20) gives H;R-1HX + Mxx H;R-1Hp + Mxp][X+] [ H:R-1Hx + Mpx H:R-1Hp + Mpp p+
=
[H;R-1y + Mxxx- + MxpP-] H:R-1y + Mpx x - + Mppp-
(28)
Reference [5] shows that the solution for the state estimate is X+
=
where
(p;H; + Px;H:) R-1y + (P~xx + Px;Mpx)x- + (P~xp + Px;Mpp)p
(29)
P = P- and (30) (31)
which are the updated state and cross-covariances, respectively. This means that and the given a set of m measurements, 51, a priori estimates of the states, parameters, P, and their associated covariance, P;', it is possible to find an updated value of the states from equation (29). There are instances, however, where all of this information is not available a priori and an alternate solution must be formed. The next few sections investigate these alternatives.
x-,
Woodbury, Majji, and Junkins
466
Special Case 1: No Parameter Estimates A Priori The analysis presented in the previous section relied on the assumption that a priori estimates of both the states and parameters are available. Depending on the desired implementation, either only state estimates or parameter estimates may be available. Consider first the case where only state estimates are known a priori. In other words, there is no knowledge on the possible values of the measurement model parameters. Mathematically, this means Ppp ~ 00 or p;} ~ 0 which forces Mxp = Mpx = Mpp = o from equations (25), (26), and (27), respectively. Additionally, Mxx reduces to p;.-l from equation (24). Thus, equation (29) becomes (32)
and the updated covariances are now
p+ xx
=
1 + HTR- 1 H - HTR- 1H (HTR- 1 H)-I HTR- 1H)-I (P_xx x x x pp p p x
(33)
Special Case 2: No State Estimates A Priori As an alternative to the developments above, consider the case where there are no a priori estimates for the states but knowledge is available for the measurement model parameters. In this scenario, Pxx ~ 00 or p;.1 ~ 0 which forces Mxx = Mxp = Mpx = oand Mpp = Pp; from equations (24) to (27). Substituting these values into equation (29) results in
i+
=
(p+ HT + p+ HT)R-1y- + (p+ P-I)pA xxx xpp xppp
(35)
where the updated covariances equate to
p+ HTRxx = (HTRx 1Hx - HTRx 1Hp(HTRp 1Hp + p-I)-I pp p 1H)-I x
(36)
By invoking the Sherman-Morrison-Woodbury Matrix Inversion Lemma (described in Appendix B) with
= p-PP'I B = HTP' C = R- 1, D = HP the coefficient in front of p in equation (35) reduces to p+ xx HTRx 1Hp(HTRp 1Hp + p-I)-Ip-I pp pp = p+ xx HT(H x p'P T-IT + R)-I Hp A
pp'~p
(38)
(39)
and the solution for the updated state estimate becomes
i+ = (p;H;
+ Px;H!)R-Vy -
p;H;(H~p~!
+ R)-I Hpp
(40)
Special Case 3: No A Priori Estimates In the case where no a priori information is available for either the states or the parameters then Pxx ~ 00 or p;.1 ~ 0 and Ppp ~ 00 or p;} ~ O. By substituting in the former to equation (32) and the latter into equation (40) both equations reduce to
i+
=
(p+ HTx + p+ HT)R-1yxx xp p
(41)
Measurement Model Parameter Errors
467
where
under these initial conditions. Comparing equations (41) to (43) to equations (13), (14), and (17) makes it apparent that the batch consider least squares solution found previously is also the minimum variance consider least squares result if W = R- 1 • This implies that the optimal estimate of the states given no prior knowledge from both a least squares and a minimum variance perspective is given by equations (41) to (43). Special Case 4: True Parameter Values Known If the true parameter values are known then p = p and Ppp = Pxp Substituting this into equations (36) and (37) reduces P:; and Px; to
= Ppx = O.
1 H)-l p+ = 0 P-xp = (HTRx x'xp
because (H;R-1Hp
+
(44)
p;pl)-l ~ O. Equation (40) is now
1(-Y- H pP ) H x)-1 HTRx = (HTR-1 x x
(45)
A
which is just the traditional least squares solution with W -=I R- 1 as shown in equation (7). Furthermore, under this condition Mxx = P:X and equation (29) becomes (46)
which is equivalent to Equation (19).
Sequential Estimation Although the batch results given in the previous section provide theoretical insights into the nature of the consider least squares framework, batch estimators are often unwieldly for real world computations because of either very large data sets or limited computational power. It would be convenient to develop a sequential estimator based on the previously developed batch estimators. Least Squares
Beginning again with the augmented state vector, least squares solution is known to be [6]
Z,
the sequential or recursive (47)
where (48) (49)
Although the a posteriori and a priori notation used is more natural for dynamic systems in which state estimates are updated once new measurements become available, for static systems a-can be thought of as the estimate at the h step and a + is the estimate at the k + 1 step.
e
Woodbury, Majji, and Junkins
468
By mUltiplying out the block matrices of equations (47) and (48), the updated state estimate is shown to be (50) where
(52) (53) Minimum Variance
The minimum variance sequential estimator is found by beginning with equation (20) and using the matrix inversion lemma with A=p_-I B=HTz' C=R- 1, D=Hz z'
(54)
to produce (55) Simplifying yields
z+ = z- + p-;Hi(H~;H;' + R)-l(y Expanding and solving for
x+
Hzz-)
(56)
finds the minimum variance result
x+ = x- + K(y -
Hxx- - HiJ)
(57)
where K
= (P;.Jl; + P;/f!) (HxP;.Jl; + HxP;/f! + H~;ft; + H~p/f! + R)-l (58)
A verification of this result can be achieved by minimizing the trace of the update state covariance, Pxx +, as shown in Appendix A. After comparing equations (57) and (58) to equations (50) and (51), it is apparent that the minimum variance sequential solution is equivalent to the sequential consider least squares result if W = R- 1 as before. Additionally, equations (52) and (53) are still used to update the state and cross-covariances, respectively. It is also possible to update the parameter estimates using the complete augmented state vector, however the consider least squares approach assumes that the parameter estimates, p, and their associated covariance, PPP' do not change from their initial a priori estimates.
Examples The results described above will be demonstrated through the use of two examples, one where the system is static, and the other will have known dynamics that can be described discretely via (59)
469
Measurement Model Parameter Errors
TABLE 1.
Values Used in the Static Example
Variable Value
4
4.2
p
13
Pxxo
0.4
0.3
0.04
o
0.01
0.01
100
where k is the step number and is the state transition matrix with k = (k + 1, k). Static Example
The static system example demonstrates the capabilities and limitations of the methods developed in the previous sections. It is assumed that two sensors are available to provide measurement data of a stationary object. One sensor has an associated bias which can be represented by YI
=
X
+ P + VI
Vj ~
(60)
N(O, R I )
The second sensor can estimate the parameter value directly or (61)
It is assumed, though, that this measurement has a much higher variance than the
first measurement (R 2 »R j ). This additional measurement makes the augmented system completely observable. If the augmented state vector was not fully observable then singularities would occur in equation (41). Table 1 contains the values for the initial true and estimated values used in the analysis and following discussion. Table 2 contains the results of a single test using the same 1000 sets of measurements for each of the methods described above. Equation numbers are provided to help correlate the method used and its theoretical development. The results of the complete augmented system have also been included for comparison purposes. TABLE 2.
Results of the Static Example, Single Run Updated State
Updated Parameter
Traditional Least Squares (TLS) [equation (7)]
3.8962
TLS: State Estimates [equation (19)]
3.8962 4.1823
N/A N/A N/A N/A N/A N/A
Estimation Method
Consider Least Squares (CLS) [equation (41)] CLS: State Estimates [equation (32)] CLS: Parameter Estimates [equation (40)]
4.1949 3.9222
CLS: State and Parameter Estimates [equation (29)] Augmented Least Squares (ALS) [equation (11)]
3.9737 4.1823
ALS: State Estimates
4.1949
ALS: Parameter Estimates ALS: State and Parameter Estimates [equation (20)] Consider Sequential Least Squares [equation (57)]
3.9222 3.9737 3.9284
0.2139 0.2012 0.4740 0.4226
N/A
470
Woodbury, Majji, and Junkins
The first observation is that the traditional least squares (TLS) solution is off by almost the exact amount of the parameter error as a result of the linearity of the parameters and the assumption that they are perfectly known. When the other two estimators are given only state estimates a priori, the answer tends to be closer to those given values than the parameter estimates and vice versa, which is expected. Comparing the consider least squares (CLS) results to the augmented least squares results (ALS) shows that they are in fact equivalent. This is also not swprising since the CLS results are simply the state estimates extracted from the ALS solution. It should be noted, however, that this is not always true, but a result of the examples being used in this paper. The critical piece, however, is that in order to obtain these results, ALS had to invert a 2 X 2 matrix, whereas CLS only had to invert a 1 X 1 matrix. In this simple example the difference is trivial, but in larger systems considering the parameters can result in significant computational savings because a smaller state sensitivity matrix is inverted. Although not as significant, CLS also uses fewer multiplication and addition operations than the ALS. For these reasons the augmented solutions will not be explored further in this article. Although covariances of equal magnitudes were used in this example, as one covariance estimate increases with respect to the other, the state estimate will approach one of the aforementioned special cases. For example, if the initial covariance for the parameters was assumed to be 100 instead of 0.01, then an approximate solution for CLS with both initial estimates could be found using CLS with only initial state estimates. Similarly, if both initial covariances increase with respect to the measurement covariance then the solution to CLS with no initial estimates will be approached. The individual measurement results for the sequential estimator are shown in Fig. 1. Note that the state covariance is converging and the state estimates stay 0.4
I .
State Error
- - 30" Covariance Bound
0.3
0.2
j
0.1
j
o
~
1l $ -0.1
rn
.
-
-
-
?
-0.2
-0.3
-0.4
o
100
200
300
400
500
600
700
800
Test Number
FIG. 1.
Sequential Estimator Results for the Static Example.
900
1000
Measurement Model Parameter Errors
471
300r------,------,------,-----,--~~~==~~===c~
250
-0.08
-0.06
-0.04
-0.02
o
0.02
State Estimate Error
FIG. 2.
Static Example Monte Carlo Results - TLS: No Initial Estimates.
comfortably within the covariance bounds. If the initial covariance estimates were larger than those used in this example, the estimator would still eventually converge, but would require more measurements. To compare the updated state covariance value for each method, a Monte Carlo analysis was performed using 1000 simulations with 1000 pairs of measurements per simulation. Figures 2-7 display the results. Each figure contains a histogram of the results combined with the estimated 30" covariance bounds for each method. Although each method produces a seemingly Gaussian distribution for the results, there are some discrepencies as to how the data points fall within the covariance bounds. Not surprisingly, there is a clear bias caused by the parameter error in both TLS methods that produces a distribution outside of the covariance bounds. There is also a bias, however, in all three cases where a priori estimates were provided to the CLS framework. This is because of the fact that in all cases a single value was given to the initial state and parameter estimates. If this value was allowed to vary via a Gaussian distribution with the true value as its mean and covariance given by their respective initial values, this bias would disappear. Thus, error in the initial estimates produces a bias in the resulting estimate during batch estimation. Additionally, the initial covariance estimates must be large enough to anticipate this error otherwise a large enough bias will place the mean estimate outside of the covariance bounds. The CLS result with no initial estimates shows no bias from the true values, but at the cost of a much larger updated covariance than any of the other methods. Furthermore, the fact that the result of this method from a single run (Table 2) was close to the initial estimate was just a coincidence and not indicative of the process as a whole.
472
Woodbury, Majji, and Junkins
250
iz
100
50
-0.1
-0.08
-0 . 06
-0.04
o
-0.02
0.02
State Estimate Error
FIG. 3. Static Example Monte Carlo Results - TLS: State Estimates.
Dynamic Example
A linear oscillator is used to examine how the Illimmum variance batch estimators presented above can be used on dynamic systems. The dynamic equation for the undamped oscillator is given by
200
8
~
150
o'"'"
....o
i...
100
Z
50
0'------"--1
-1.5
-0.5
o
0.5
1
State Estimate Error
FIG. 4.
Static Example Monte Carlo Results - CLS: No Initial Estimates.
1.5
473
Measurement Model Parameter Errors
250r------.--~--_.------_r------._------._------r_--r_~
_
State Error
- - 30' Covariance Bound 200
....o
i
100
Z
50
O~----~---L--~------~~
-0.8
-0.6
FIG. 5.
-0.4
-0.2
0
0.2
0.4
0.6
State Estimate Error Static Example Monte Carlo Results - CLS: State Estimates.
x+w;x=o
(62)
where Wn is the natural frequency of the system. In state-space form these equations are written as
200
i
150
o...." o
i
100
Z
50
OL-----~L-----~-
-0.4
FIG. 6.
-0.3
-0.2
-0.1
0
0.1
0.2
State Estimate Error Static Example Monte Carlo Results - CLS: Parameter Estimates.
0.3
Woodbury, Majji, and Junkins
474
200
00
8
iil ~
15 0
8
i
100
Z
50
OL-____ -0.4
FIG. 7.
-L~L_
-0.3
_ _L __ _ _ __
-0.1 o State Estimate Error
-0.2
0.1
0.2
0.3
Static Example Monte Carlo Results - TLS: State and Parameter Estimates.
. = [Xl] Xz
X
=
[0-w~ °l][Xl]Xz
= Ax
(63)
Because this system is Linear-Time Invariant (LTI), the State Transition Matrix (STM) is known to be
(64)
where dt = t - to and (65)
Two measurements are available to monitor the states. The first is a position measurement subject to only white noise (66)
The second is a velocity measurement subject to a constant bias and white noise Yz = Xz
+ b + 1-'2
1-'2 - N(O, Rz)
(67)
Based on these two measurements, three different scenarios are examined. The fIrst uses only the unbiased position measurements. The second uses both measurements, but a Traditional Least Squares approach is applied. The third scenario uses both measurements and a Consider Least Squares framework to estimate the solution. For each scenario, the initial values of the states are the solve-for parameters. Given that each set of measurements is taken at equal time steps, the measurements can be back-propagated by
475
Measurement Model Parameter Errors
TABLE 3.
Values Used in the Dynamic Example
2.3
Value
Yk =
3
0.2
0.0 0.04 0.08
0.49
0.04
0.0016
0.01
0.01
[~::] = [~ ~]k(Llt)[~~] + [~]b + [~] = Hxk(Llt)xo + Hpb + v (68)
because this is an LTI system. The values used in this scenario are shown in Table 3. Values not shown are set to zero. Note that the bias is within the measurement noise of the sensors. A 1000-run Monte Carlo test was performed for all three scenarios. 100 values of each measurement were collected over a lOs time interval for each run. A priori estimates were provided for both intial states as well as the bias, b. The means and standard deviations for each state and scenario are shown in Table 4. Comparing scenario's 1 and 2 shows that introducing the second measurement improved the resulting covariance, but introduced a bias into the estimates. Despite the bias being smaller than measurement noise it still produces a bias in the resulting estimates. This is also shown by the graphs in Figs. 8 and 9. These figures plot a histogram of the initial position estimates from each Monte Carlo run. The estimated 3a covariance bounds have also been plotted. Using Consider Least Squares, however, shows that not only is covariance reduced as in Scenario 2, but the bias from the estimates is also removed. Figure 10 shows the histogram for the CLS position estimates. Similar results are seen for the initial velocity estimates, but are not as pronounced as those for the initial position.
Conclusions Consider estimation provides a middle ground between ignoring parameter error and completely accounting for it. Provided that the augmented state vector including both states and parameters is fully observable, the consider framework can be used to account for the parameter error without directly estimating any or all of the parameters from both a least squares and minimum variance analysis. Futhermore, by accounting for the parameter error, the consider approach provides a rigorous path to improve state estimation through TABLE 4.
Means and Standard Deviations from a lOOO-run Monte Carlo Simulation Scenario 1
Scenario 2
Scenario 3
Mean S.D.
-0.0002 0.0131
-0.0072 0.0099
0.0004 0.0100
Mean S.D.
-0.0014 0.0143
-0.0027 0.0102
-0.0004 0.0102
Xl
x2
476
Woodbury, Majji, and Junkins
250
i tl
200
o
150
iz
100
'0
50
OL----'-'---0.05 -0.04 -0.03
-0.02
-0.01
o
0.01
0.02
0.03
0.04
0.05
Postion Estimate Error
FIG. 8.
Dynamic Example Monte Carlo Results Initial Position Estimates - Scenario 1.
the reduction of both state error and variance. While using the augmented state vector to estimate both states and parameters may further improve those estimates, the consider estimation framework is an alternative for complex and
250
200
i u
0150
""o
iz
100
50
-0.03
-0.02
-0.01
o
0.01
0.02
0.03
0.04
Position Estimate Error
FIG. 9.
Dynamic Example Monte Carlo Results Initial Position Estimates - Scenario 2.
Measurement Model Parameter Errors
477
300r-----~----,------,-----,------~~~~==~~====~
250
200
i
2 ~ o
150
Z 100
50
0'---------......- -
-0.04
-0.03
-0.02
-0.01
o
0.01
0.02
0.03
0.04
Position Estimate Error
FIG. 10. Dynamic Example Monte Carlo Results: Initial Position Estimates - Scenario 3.
computationally intensive systems and provides a well justified path for parameter order reduction.
References [1]
[2] [3] [4] [5] [6]
MILLER, J., KONOPLIV, A., ANTREASIAN, P., BORDI, J., CHESLEY, S., et al. "Detennination of Shape, Gravity, and Rotational State of Asteroid 433 Eros," Icarus, Vol. 155, No.1, 2002, pp. 3-17. SCHMIDT, S.P. "Application of State-Space Methods to Navigation Problems," Advances in Control Systems, Vol. 3, 1966, pp. 293-340. JAZWINSKI, A.H. Stochastic Processes and Filtering Theory, Academic Press, Inc., 1970. BIERMAN, GJ. Factorization Methods for Discrete Sequential Estimation, Mineola, NY, Dover Publications, Inc., 1977. TAPLEY, B., SHUTZ, B., and BORN, G. Statistical Orbit Determination, Academic Press, 2004. CRASSIDIS, J. and JUNKINS, J. Optimal Estimation of Dynamic Systems, Boca Raton, FL, Chapman & HallnCRC Press, 2004.
Appendix A - Minimum Variance Gain Alternate Derivation Using equation (57) to evaluate the updated covariance generates P; = P:X - K(HxP:X + HPP;x) - (P:xH; + p";/f~)KT + K(HxP:xH; + HxP;/f~
+ Hpp;ft; + Hppp/f~ + R)KT (69) The optimal gain is found by minimizing the cost function (70)
478
Woodbury, Majji, and Junkins
where tr (A) denotes the trace operation on the matrix A. Taking the partial derivative of the above cost function with respect to the gain, K, and using the trace derivative properties found in Appendix B results in
aJ
aK = 0
= 2K(HJ>;)I; + HxY;/f; + H,J>;fi; + H,J>p/f; + R) - 2(P;)!; + P;/f;)
(71)
+ P;/f;) (HxP;)!; + HJ>;/f; + H,J>;fi; + H,J>p/f; + R)-l
(72)
Solving for the gain yields
K = (P;)!;
which is equivalent to the result shown in equation (58).
Appendix B - Useful Matrix Properties Matrix Trace Calculus
Taking the partial derivative of the trace of matrix is used frequently in Kalman filter derivations to find the minimum variance solution for the gain. Some useful derivatives are given by
a
-tr(KA) =AT
aK
(73)
(74)
(75)
where K and A are two arbitrary matrices satisfying matrix multiplication rules. Sherman-Morrison-Woodbury Lemma
The Sherman-Morrison-Woodbury matrix inversion lemma is used in Kalman filter derivations to define the gain in terms of the propagated covariances instead of the updated covariances. Let F = [A
+ BCD]-l
(76)
where F, A, B, C, and D are all arbitrary matrices satisfying matrix multiplication rules. Assuming all inverses exist it can be shown that (77)
Proofs of this can be found in multiple locations, one such being reference [6].
