## 2 Theory
### Kalman Filter
Because there is no external control vector in the proposed system, and based on the above definitions and assumptions, the Kalman filter time update equations are:  
\[X(k|k-1)=A\times X(k-1|k-1)+W \tag{9}\]  
\[P(k|k-1)=A\times P(k-1|k-1)\times A^{T}+Q \tag{10}\]  
Figure 5: The process of predicting and updating Kalman filter. Update (measurement update): Combined with the predicted value and measured value, the optimal estimated state vector \(X(k|k)\) and posterior covariance matrix \(P(k|k)\) of the system at the \(k\) moment can be calculated using measurement update equations:  
\[K_{g}(k)=\frac{P(k|k-1)\times H^{T}}{H\times P(k|k-1)\times H^{T}+R} \tag{11}\]  
\[X(k|k)=X(k|k-1)+K_{g}(k)\times(Z(k)-H\times X(k|k-1)) \tag{12}\]  
\[P(k|k)=\left(I-K_{g}(k)\times H\right)\times P(k|k-1) \tag{13}\]  
where \(K_{g}\) is called the Kalman gain. Furthermore, Kalman gain plays an important role in its performance and it actually has many forms under different conditions. Based on the assumptions in this paper, the most popular and the most widely used form is applied, which is named optimal Kalman gain. Other forms of Kalman gain are not applicable because they do not agree with the hypothesis of the proposed system. For simplicity, the term \"Kalman gain\" and the term \"optimal Kalman gain\" are not differentiated in this paper, which means that \"optimal\" can be omitted, and both are unified as Kalman gain \(K_{g}\). In addition, considering the expression characteristics of Kalman gain \(K_{g}\), error covariance matrix formula, and relationships between \(P\), \(Q\), and \(R\), it can be concluded that under the premise that the observation covariance \(Q\) is fixed, the smaller the measurement error covariance \(R\), the actual measurement is trusted more, while the predicted measurement is trusted less. In other words, the value of \(Q\) determines the weight of the actual measurement, and \(R\) determines the weight of the predicted measurement. Besides, what needs to be considered is that the tracking result of single Cam-Shift algorithm would be quite inaccurate when there are severe similar color disturbances, leading to relatively large errors of the final weighted result even when the weight of the actual measurement is small. Thus, in consideration of the properties of the Cam-Shift algorithm, in order to enhance robustness and minimize the error caused by interferences, values of \(Q\) should be designed to be relatively small and values of \(R\) should be designed relatively large. Therefore, based on the definitions and assumptions above, and under the experimental environment of the proposed system, the detailed steps of Kalman filter algorithm are as follows:  
1. Initialize the Kalman filter: The state vector of the target signal source LED is a four-dimensional vector, including the coordinate of the center position of the target and the velocity component in the directions of \(x\) and \(y\), shown in Equation (14). Initialize the state transition matrix \(A\), measurement matrix \(H\), covariance matrix of motion noise \(Q\), and covariance matrix of measurement noise \(R\) as: \[X=\left[ \begin{array}{c}x\\ y\\ v_{x}\\ v_{y}\end{array} \right]\] (14)  
\[A=\left[ \begin{array}{cccc}1&0&1&0\\ 0&1&0&1\\ 0&0&1&0\\ 0&0&0&1\end{array} \right] \tag{15}\]  
\[H=\left[ \begin{array}{cccc}1&0&0&0\\ 0&1&0&0\end{array} \right] \tag{16}\]  
\[Q=\left[ \begin{array}{cccc}10^{-5}&0&0&0\\ 0&10^{-5}&0&0\\ 0&0&10^{-5}&0\\ 0&0&0&10^{-5}\end{array} \right] \tag{17}\]\[R=\left[ \begin{array}{cc}10^{-1}&0\\ 0&10^{-1}\end{array} \right] \tag{18}\]  
Suppose that the central position coordinate of the target signal source LED is \((x_{0},y_{0})\) in the original frame, then initialize posterior covariance matrix \(P\) and state vector \(X\) of the target of the original frame as:  
\[P=\left[ \begin{array}{cccc}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{array} \right] \tag{19}\]  
\[X=\left[ \begin{array}{c}x_{0}\\ y_{0}\\ 0\\ 0\end{array}