# Design and Implementation of a Two-Wheeled Self-Balancing Car Using a Fuzzy Kalman Filter
## 3 Fuzzy Kalman Filtering Algorithm Design
### Fuzzy Kalman Filtering Algorithm Design  
This section first introduces the Kalman filtering algorithm for filtering the angle and angular velocity of the self-balancing car, followed by the establishment of the fuzzification and defuzzification processes of process noise and measurement noise based on the fuzzy theory. The covariance of process noise and measurement noise experienced by the two-wheeled self-balancing car is:  
\[ \begin{cases}q(k)=\alpha(k)\cdot q\\ r(k)=\beta(k)\cdot r\end{cases}  \tag{7}\]  
where \(q(k)\in\mathbb{R}\) and \(r(k)\in\mathbb{R}\) respectively, represent the covariance of process noise and measurement noise at time instant \(k\), while \(\alpha(k)\in\mathbb{R}\) and \(\beta(k)\in\mathbb{R}\) are used to adjust the parameters of measurement noise and process noise covariance. The Kalman filter, as a linear optimal estimation algorithm, utilizes a \"predict-measure-correct\" pattern to eliminate random disturbances of the system by updating the measurement values, thus optimizing the restoration of the system's state. The Kalman filter has two main functions, namely prediction and correction, as shown in Figure 4. Prediction involves the time update equation, while correction involves the measurement update equation. The time update equation estimates the a priori estimate and covariance of the state variables at the current time by evaluating the state value at the previous time, while the measurement equation calculates the posterior estimate by combining the a priori estimate with the current measurement value, achieving the purpose of correction. A prior estimation of the angle and angular velocity of the two-wheeled self-balancing car is designed as follows:  
\[ \begin{bmatrix}\hat{\theta}(k)\\ \hat{\theta}_{b}(k)\end{bmatrix} = \begin{bmatrix}1&-T\\ 0&1\end{bmatrix}\begin{bmatrix}\hat{\theta}(k-1)\\ \hat{\theta}_{b}(k-1)\end{bmatrix} + \begin{bmatrix}T\\ 0\end{bmatrix} \hat{\theta}^{\prime}(k-1), \tag{8}\]  
\[\hat{\theta}_{c}(k)=\hat{\theta}(k) \tag{9}\]  
where \(\hat{\theta}(k),\hat{\theta}^{\prime}(k),\hat{\theta}_{c}(k)\) and \(\hat{\theta}_{b}(k)\) are, respectively, the estimated values of the car's angle, angular velocity, accelerometer angle, and static error. The accelerometer angle obtained from the prior estimate needs to be corrected through posterior estimation. The covariance matrix of the posterior estimate is defined as \(P(k)\)[19], yielded using the following equation:  
\[P(k)=(I-K(k-1)H)\Big{(}AP(k-1)A^{T}+Q(k-1)\Big{)},K(k)= \begin{bmatrix}K_{1}(k)\\ K_{2}(k)\end{bmatrix} = \begin{bmatrix}\frac{p_{11}(k)}{p_{11}(k)+r(k)}\\ \frac{p_{21}(k)}{p_{11}(k)+r(k)}\end{bmatrix} ,\]  
\[\theta_{\text{c}}^{f}(k)=\hat{\theta}_{c}(k)+P(k)(\theta_{c}(k)-\hat{\theta}_{ c}(k)), \tag{10}\]  
where \(I\) is the unit matrix, \(K(k)\) is the Kalman gain vector, \(H= \begin{bmatrix}1&0\end{bmatrix} ,\ A= \begin{bmatrix}1&-T\\ 0&1\end{bmatrix} \), \(Q(k-1)= \begin{bmatrix}q(k-1)&0\\ 0&q(k-1)\end{bmatrix} ,P(k)= \begin{bmatrix}p_{11}(k)&p_{12}(k)\\ p_{21}(k)&p_{22}(k)\end{bmatrix} \), and \(\theta_{\text{c}}^{f}(k)\) represents the corrected accelerometer angle, which is also the final output of the filter. Two time-varying parameters, \(\alpha(k)\) and \(\beta(k)\) are selected as the outputs of the fuzzy control; the noise covariance matrix of the filter is modified by adjusting the values of \(\alpha(k)\) and \(\beta(k)\). Let \(e_{\theta}(k)=\theta_{c}(k)-\theta_{c}^{f}(k)\) be defined as the residual, indicating the extent to which the self-balancing car system relies on measurement values. The variance and mean of the residual can be used to assess the performance of the filter, with the mean and variance of residual are computed as follows:  
\[\overline{e}(k)=\frac{1}{n}\underset{k=m-n}{\sum}e_{\theta}(k), \tag{11}\]  
\[\hat{P}_{e}(k)=\frac{1}{n}\underset{k=m-n+1}{\sum}e_{\theta}(k)^{2} \tag{12}\]  
where \(n\in\mathbb{R}\) represents the statistical time length, \(m\in\mathbb{Z}^{+}\). The mean and variance of the residual are chosen as inputs for fuzzy control.