如今，隨著航天軍事和自動駕駛的快速發展，卡爾曼濾波常用於即時物件偵測與追蹤。然而，實際情況下，狀態噪聲協方差矩陣和測量噪聲協方差矩陣都是未知的，而且會隨著時間作改變，使得卡爾曼濾預測的精準度下降。因此，如何有效並快速估測成為重要問題。
在過去的幾年中提出的許多方法主要可以分為以下四大類，包括 Bayesian、maxi-mum likelihood、covariance matching 和 correlation techniques。在此領域中，Autocovari-ance least square (ALS) 是常用的方法，然而將這個使用於高維度的問題會花費大量時間，進而使卡爾曼濾波難以即時預測。本論文中，結合了 Mehra 和 David L. Kleinman 的方法相結合，並通過 SDA 進行修改，除了可大幅提升計算速度，還能維持噪聲協方差矩陣的對稱半正定性質。最後，在我們的實驗結果中，我們可以發現修改後的方法比 ALS 快 2 至 4 倍左右。

Nowadays, with the rapid development of aerospace military and autonomous vehicles, Kalman filtering is commonly used for real-time object detection and tracking. However, in practice, both the state noise covariance matrix and the measurement noise covariance matrix are unknown and change over time, making the accuracy of Kalman filtering prediction degraded. Therefore, how to estimate efficiently and quickly becomes an important issue.
Many methods proposed in the past years can be divided into four main categories, including Bayesian, maximum likelihood, covariance matching, and correlation techniques. ALS) is the commonly used method, however, using this method for high-dimensional problems can take a lot of time, which makes the Kalman filtering difficult to predict in real time. In this paper, we combine the method of Mehra and David L. Kleinman, and modify it by SDA, which not only improves the computational speed significantly, but also maintains the symmetric semi-normal nature of the noisy covariance matrix. Finally, in our experimental results, we can find that the modified method is about 2 to 4 times faster than ALS.

Contents
1 Introduction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
2 Structure-Preserving Double Algorithms for Discrete and Continuous Time
Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .3
2.1 Algebraic Riccati Equation
2.1.1 Discrete Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.2 Continuous Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2 Structure preserving double Algorithm for DARE . . . . . . . . . . . . . . . . . . . . . . . 5
2.2.1 Doubling transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.2 How to Find M∗, L∗ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .7
2.2.3 Algorithm 2.1 (SDA for DARE) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .10
2.3 SDA for CARE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3.1 Transfer CARE form to DARE form . . . . . . . . . . . . . . . . . . . . . . . . . . ..11
2.3.2 Algorithm 2.2 (SDA for CARE) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .13
3 Estimation of noise covariances in Kalman filter using structure preserving
doubling algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .14
3.1 The dynamics system and Kalman filter algorithm . . . . . . . . . . . . . . . . . . . . . .14
3.1.1 Kalman Filter algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .14
3.2 Suboptimal Filter and steady state Kalman filter gain . . . . . . . . . . . . . . . . . . . 15
3.3 Innovation sequence for a suboptimal Kalman filter . . . . . . . . . . . . . . . . . . . . 16
3.4 Estimating the optical Kalman filter gain Kop . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.5 Estimating the measurement noise covariance matrix R . . . . . . . . . . . . . . . . . 19
3.6 Estimating the state noise covariance matrix Q . . . . . . . . . . . . . . . . . . . . . . . . 20
3.7 Autocovariance least square . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .21
3.8 Experiment result and discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4 Computationally Efficient SDRE Control Design for double pendulum inverted
on a
Cart. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..31
4.1 State dependent Riccati equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.2 The double pendulum inverted on a cart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32
4.3 Experiment result and discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ..40
5 Nonlinear Kalman Filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .45
5.1 Extended Kalman Filter (EKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
iv
5.2 Unscented Kalman Filter (UKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .49
5.3 State-Dependent Riccati Equation Filter (SDREF) . . . . . . . . . . . . . . . . . . . . . .51
5.4 Experiment result and discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .59

Reference
[1] R. E. Kalman, ‘‘New methods and results in linear prediction and filtering
theory,”Proc. Symp. on Engineering Applications of Random Function Theory and
Probability.New York:Wiley, 1961.
[2] R. E. Kalman and R. S. Bucy, “New results in linear filtering and prediction
theory,”Trans. ASME, J . Basic Engrg., ser.I), vol. 83 , pp. 95-108, March 1961.
[3] D. Alspach. A parallel filtering algorithm for linear systems with unknown
timevarying noise statistics. IEEE Trans. Auto. Cont., 19(5):552–556, 1974.
[4] C. Hilborn and D. Lainiotis. Optimal estimation in the presence of unknown
parameters.IEEE Trans. Systems, Science, and Cybernetics, 5(1):38–43, 1969.
[5] T. Bohlin. Four cases of identification of changing systems. In R. Mehra and D.
Lainiotis,editors, System Identification: Advances and Case Studies. Academic Press,
1st edition, 1976.
[6] R. Kashyap. Maximum likelihood identification of stochastic linear systems. IEEE
Trans. Auto. Cont., 15(1):25–34, 1970.
[7] K. Myers and B. Tapley. Adaptive sequential estimation with unknown noise
statistics.IEEE Trans. Auto. Cont., 21:520–523, 1976
[8] R. Mehra. On the identification of variances and adaptive Kalman filtering. IEEE
Trans. Auto. Cont., 15(12):175–184, 1970.
[9] R. Mehra. Approaches to adaptive filtering. IEEE Trans. Auto. Cont., 17:903–908,
1972.
[10] Odelson, Brian J., Murali R. Rajamani, and James B. Rawlings. "A new
autocovariance least-squares method for estimating noise covariances." Automatica
42.23(2006): 303-308.
[11] Lingyi Zhang, David Sidoti, Adam Bienkowski, Krishna R. Pattipati,Yaakov Bar-
61
Shalom, David L. Kleinman, On the Identification of Noise Covariances and Adaptive
Kalman Filtering: A New Look at a 50 Year-Old Problem. IEEE, March 2020.
[12] A. Laub, “A schur method for solving algebraic riccati equations,” IEEE
Trans. Autom. Contr., vol. 24, no. 6, pp. 913–921, 1979.
[13] R. Patel, Z. Lin, and P. Misra, “Computation of invariant subspaces
corresponding to stable eigenvalues of hamiltonian matrices,” in Proc.
Amer. Control Conf., 1990, pp. 2565–2571
[14] Eric Chu, HY Fan, Wen-Wei Lin, and C.-S. Wang, Structure-Preserving Algorithms
for Periodic Discrete-Time Algebraic Riccati Equations. International Journal of Control
May 2004.
[15] Eric Chu, HY Fan, and Wen-Wei Lin, A structure-preserving doubling algorithm for
continuous-time algebraic Riccati equations. Linear Algebra and its
Applications ·February 2005
[16] Ç imen, Tayfun. "State-dependent Riccati equation (SDRE) control: a survey." IFAC
Proceedings Volumes 41.2 (2008): 3761-3775.
[17] L. -G. Lin and W. -W. Lin, "Computationally Efficient SDRE Control Design for 3-
DOF Helicopter Benchmark System," in IEEE Transactions on Aerospace and
Electronic Systems, vol. 57, no. 5, pp. 3320-3336, Oct. 2021,
doi:10.1109/TAES.2021.3074211.
[18] Sultan, Ghassan A., and Ziyad K. Farej. "Design and Performance Analysis of LQR
Controller for Stabilizing Double Inverted Pendulum System." Circ. Comput. Sci. 2.9
(2017): 1-5.
[19] Bogdanov, Alexander. "Optimal control of a double inverted pendulum on a
cart." Oregon Health and Science University, Tech. Rep. CSE-04-006, OGI School of
Science and Engineering, Beaverton, OR (2004).4
[20] Crowe-Wright, Ian JP. "Control Theory: The Double Pendulum Inverted on a
Cart."(2018).
[21] Wan, Eric A., and Rudolph Van Der Merwe. "The unscented Kalman filter." Kalman
filtering and neural networks (2001): 221-280.
[22] Wan, Eric A., and Rudolph Van Der Merwe. "The unscented Kalman filter for
62
nonlinear estimation." Proceedings of the IEEE 2000 Adaptive Systems for Signal
Processing, Communications, and Control Symposium (Cat. No. 00EX373). Ieee, 2000.
[23] Cloutier, James R. "State-dependent Riccati equation techniques: an
overview." Proceedings of the 1997 American control conference (Cat. No. 97CH36041).
Vol. 2. IEEE, 1997.
[24] Nemra, Abdelkrim, and Nabil Aouf. "Robust INS/GPS sensor fusion for UAV
localization using SDRE nonlinear filtering." IEEE Sensors Journal 10.4 (2010): 789-798.
[25] Julier, Simon J. "The scaled unscented transformation." Proceedings of the 2002
American Control Conference (IEEE Cat. No. CH37301). Vol. 6. IEEE, 2002
[26] Çimen, Tayfun, and A. Osman Merttopçuoğlu. "Asymptotically optimal nonlinear
filtering: Theory and examples with application to target state estimation." IFAC
Proceedings Volumes 41.2 (2008): 8611-8617.
[27] Ge, Ming, and Eric C. Kerrigan. "Noise covariance estimation for time-varying and
nonlinear systems." IFAC Proceedings Volumes 47.3 (2014): 9545-9550.
[28] Rajamani, Murali R., James B. Rawlings, and Tyler A. Soderstrom. "Application of
a new data-based covariance estimation technique to a nonlinear industrial blending
drum." Texas-Winsconsin Modeling and Control Consortium, Tech. Report 3 (2007).