# Highly Curved Lane Detection Algorithms Based on Kalman Filter
## 2 Research Method
### Curve Lane Detection Based on Kalman Filter and Circle Equation  
For the curve line, the second-best fit equation is the circle equation, shown in Figure 7. In this part, we consider curve line detection algorithms [24] based on the Kalman filter and circle equation. From the circle equation \(r^{2}\ =\ (x-h)^{2}+(y-k)^{2}\) we need to define circle radius \(r\) and the center of circle \((h,k)\). Using every three points of a circle we can draw a pair of chords. Based on these two chords we can calculate the center of the circle [36]. If we have \(n\) number points, it is possible to calculate \(n-2\) number center. Pairs of chords in each chain are used to calculate the center. Here, \((x_{1},y_{1})\), \((x_{2},y_{2})\), \((x_{3},y_{3})\) points divide a circle to three arcs, and \(L_{1},L_{2}\) are the perpendicularly bisecting lines of the corresponding  
Figure 5: Left and right turning on the road. chords. \(P_{1},P_{2}\) points, shown in Figure 8[36], are located in the lines \(L_{1},L_{2}\), and the Equations (18) and (21) show the coordinates of \(P_{1},P_{2}\) points. \[\left\{ \begin{array}{ll}P_{1(x)}\ =\ (x_{1}+x_{2})/2\\ P_{1(y)}\ =\ (y_{1}+y_{2})/2\end{array} \right. \tag{18}\]  
Perpendicular lines rules and \(P_{1}\) points coordinate are used to calculate the equation of \(L_{1}\) line in Equations (19) and (20)  
\[y\ =\ m_{1}x+c_{1}\ \ \ m_{1}\ =\ \frac{y_{1}-y_{2}}{x_{1}-x_{2}}\ \ \ m_{11}\ =\ - \left(\frac{1}{m_{1}}\right) \tag{19}\]  
\[c_{11}\ =\ P_{1(y)}-P_{1(x)}m_{11}\ \ \ y\ =\ m_{11}x+c_{11} \tag{20}\]  
For the \(L_{2}\) line same calculation runs to estimate the equation of \(L_{2}\) line, expressed in form of Equations (22) and (23). \[\left\{ \begin{array}{ll}P_{2(x)}\ =\ (x_{2}+x_{3})/2\\ P_{2(y)}\ =\ (y_{2}+y_{3})/2\end{array} \right. \tag{21}\]  
\[y\ =\ m_{2}x+c_{2}\ \ \ m_{2}\ =\ \frac{y_{2}-y_{3}}{x_{2}-x_{3}}\ \ \ m_{22}\ =\ - \left(\frac{1}{m_{2}}\right) \tag{22}\]  
Figure 6: Relationship between “\(a\)” parameter and road turning. \[c_{22}\ =\ P_{2(y)}-P_{2(x)}m_{22}\quad y\ =\ m_{22}x+c_{22} \tag{23}\]  
Based on \(L_{1},L_{2}\) lines we can calculate the center of the circle expressed in Equations (24) and (25). The intersection of these two lines indicates the center of the circle. \[\left\{ \begin{array}{l}y_{center}\ =\ m_{22}x_{center}+c_{22}\\ y_{center}\ =\ m_{11}x_{center}+c_{11}\end{array} \right. \tag{24}\]  
\[x_{center}\ =\ \frac{c_{22}-c_{11}}{m_{11}-m_{22}}\quad y_{center}\ =\ m_{11}x_{center}+c_{11} \tag{25}\]  
Figure 8: Calculation of the center of the circle. Figure 7: Circle model of the road turning. But, this method cannot determine the center correctly, it is easily disturbed by noises. Therefore, we need the second step for the estimation of the correct center using a Kalman filter. The Kalman filter is estimated using the raw data of the center \((x_{center},\ y_{center})\), which is stored by the previous step. For the \(x\) coordinate and \(y\) coordinate of the center, we need individual estimation based on the Kalman filter. Equations (26)-(32) show the Kalman filter for the center of the circle. Equations (26)-(28) show an initial value of Kalman gain and \(P_{prior}\) is the covariance of the predicted state. \[K_{x_{center}}\ =\ 1,\ I\ =\ 1,\ H\ =\ 1, \tag{26}\]  
\[X_{prior}\ =\ X_{post} \tag{27}\]  
\[P_{prior}\ =\ AP_{post}A^{T}+Q_{r}\ =\ P_{post}+Q_{r} \tag{28}\]  
\[K_{x_{center}}\ =\ (P_{prior}H^{T})\Big{(}HP_{prior}H^{T}+\mathrm{R}\Big{)}^{ -1}+Q_{r} \tag{29}\]  
\[z_{i}\ =\ x_{center(i)} \tag{30}\]  
\[X_{post}\ =\ X_{prior}+K_{x_{center}}\Big{(}z_{i}-HX_{prior}\Big{)} \tag{31}\]  
\[P_{post}\ =\ (I-K_{x_{center}}H)P_{prior}+Q_{r} \tag{32}\]  
where \(z_{i}\ =\ x_{center(i)}\) is the \(x\)-coordinate value of the center, which is stored by the previous step. After that, we can run the same process for the y coordinate value of the center. In the end, we can easily estimate the radius of the circle using these correct values of \(x\), \(y\) coordinate of center.