+ I wrote a controller for a differential drives mobile robot with Arduino in the previous post. + The controller takes two inputs \(v\) and \(w\), and generate three output which is the state of the robot \(x, y,\theta\). + The output of the odometer was not accurate due to different factors which the most important one is the wheel slipping, + encoder resolutions and the measurement accuracy. + In this post, I am going to write an extended Kalman filter to correct the odometer using other information such as LADAR reading, + IMU or radar. +
+ ++ Kalman filter is a well-known set of mathematical equations for discrete values. + The advantage of the Kalman filter is that use only the previous state to predict the current state. + To summarise, the Kalman filter is a set of mathematical equations used to estimate the future state + based on an educated guess, where the Kalman filter uses in any environment with uncertain information. +
+ ++ To estimate the current state \(x_t\) using Kalman filter there are three input required which are controller \(u_t\), correction \(z_t\) and prevoiuse state \(x_{t-1}\). +
+ + ++Starting with the model equations as shown below, which is an improved version of the old model. + The new model has two states which (1) \(D_l \neq D_r\) and the equation below shows the output +
+ + +$$ \begin{pmatrix}x^{‘} \\\ y^{‘} \\\ \theta^{‘} \end{pmatrix} = \begin{pmatrix}x \\\ y \\\ \theta \end{pmatrix} ++ \begin{pmatrix} (R + \frac w 2)(sin(\theta+\alpha) – sin(\theta) \\\ (R + \frac w 2)(-cos(\theta+\alpha) + cos(\theta) \\\ \alpha \end{pmatrix} \tag{1} \label{1} +$$ + ++ and (2) where \(D_l = D_r\) and the model become as below which is matching what we descripe in the first post. +
+ +$$ \begin{pmatrix}x^{‘} \\\ y^{‘} \\\ \theta^{‘} \end{pmatrix} = \begin{pmatrix}x \\\ y \\\ \theta \end{pmatrix} ++ \begin{pmatrix} D_r sin(\theta) \\\ D_l cos(\theta) \\\ 0 \end{pmatrix} \tag{2} +$$ + ++ Note that the steps below is applied for both cases when \(D_l \neq D_r \) and \(D_l = D_r \). +
+ ++Where in \eqref{1} \(\alpha = \frac{D_r – D_l} w\) and \(R = \frac {D_l} {\alpha} \). \(D_l\) +and \(D_r \) are the distance the robot + moved for left and right wheel respectively. By close analysis of the above + equations we know that there are two group + \(x,u\) of variable which belong to state and controller. + And these variables are belong to \(g = (x, u)\) function. + Where \(x = (x,y,\theta)\) and \(u=(D_l , D_r)\). +
+$$ \begin{pmatrix}x^ {‘} \\\ y^{‘} \\\ \theta^{‘} \end{pmatrix} = g(x,y,\theta, D_l , D_r) \tag{3} $$ + ++ The function below is used to compute the g matrix where the input is the state, and the output is G matrix 3x3. +
+ +{% highlight ruby %} +Computer G +{% endhighlight %} + + ++ The blow is the linear equation of Kalman filters where \(A_t, B_t\) are the factors of prediction and correction, respectively. + +
+$$ x_t = A_t x_{t-1} + B_t u_t$$ +$$ \overline \mu_t = A_t \mu_{t-1} + B_t u_t$$ +$$ \sum = A_t \sum_{t-1} {A_t}^T + R_t$$ + ++Because our problem is nonlinear due to the present of the rotation matrix we will take the Jacobian matrix of g which is the derivative of \(\frac {\delta g} {\delta state}\) where the state is \([x,y,\theta]^T\). Therefore \(G : \frac {\delta g_1} {\delta x} , \frac {\delta g_1} {\delta y}, \frac {\delta g_1} {\delta \theta} \) +
+ +$$ x_t = g(x_{t-1}, u_t) $$ +$$ \overline \mu_t = g(mu_{t-1}, u_t) $$ +$$ +G : \begin{pmatrix}\frac {\delta g_1} {\delta x} && \frac {\delta g_1} {\delta y}&& \frac {\delta g_1} {\delta \theta} \\\ + \frac {\delta g_2} {\delta x} && \frac {\delta g_2} {\delta y}&& \frac {\delta g_2} {\delta \theta} \\\ + \frac {\delta g_3} {\delta x}&& \frac {\delta g_3} {\delta y}&& \frac {\delta g_3} {\delta \theta} \end{pmatrix} +$$ + ++ After taking the derivative of the state, we end up with the \(G\) matrix, which show below +
+ +$$ G : \begin{pmatrix} 1 && 0&& (R + \frac w 2)(-cos(\theta+\alpha) - cos(\theta) \\\ +0&& 1 && (R + \frac w 2)(sin (\theta+\alpha) - sin(\theta)\\\ 0 && 0 && 1\end{pmatrix} +$$ ++The below code is the coding of the G where the function take the state as input and return a matrix \((3x3)\). +
++Next we need to compute controller where \(R_t = V_t \sum_{control} {V_t}^T\) where + \(\sum_{control} = \begin{pmatrix} {\sigma_{D_l}}^2 && 0 \\\ 0 && {\sigma_{D_r}}^2 \end{pmatrix} \) + is the covariance matrix (2x2), and \(V_t = \frac {\delta g} {\delta{control}} \) which is (3x2) matrix. + Therefore \(V_t\) is shown below after taking the derivative of g in relate to the control value. Note that \(\alpha = \frac{D_r – D_l} w\) + and \(R = \frac {D_l} {\alpha} \) during taking the derivative. +
+$$ V_t : \begin{pmatrix} \frac{wr} {D_r – D_l}^2 (sin(theta^{‘}) - sin(theta)) - \frac {D_r + D_l}{2(D_r-D_l)}cos(theta^{‘}) && + \frac{-wr} {D_r – D_l}^2 (sin(theta^{‘}) - sin(theta)) + \frac {D_r + D_l}{2(D_r-D_l)}cos(theta^{‘}) \\\ + \frac{wr} {D_r – D_l}^2 (cos(theta^{‘}) + cos(theta)) - \frac {D_r + D_l}{2(D_r-D_l)}sin(theta^{‘}) && + \frac{-wr} {D_r – D_l}^2 (cos(theta^{‘}) + cos(theta)) + \frac {D_r + D_l}{2(D_r-D_l)}sin(theta^{‘}) \\\ + -\frac 1 w && \frac 1 w \end{pmatrix} +$$ ++To sum up the prediction step we need to computer both covariance matrix value in state estimation and control correction \(\sum_{state} \) and \( \sum_{control}\). +
+$$ {\mu_{D_l}}^2 = (\alpha_1 D_l)^2 + (\alpha_2 (D_L – D_R))^2$$ +$$ {\mu_{D_r}}^2 = (\alpha_1 D_r)^2 + (\alpha_2 (D_L – D_R))^2$$ + ++ The code below shows the implementation of both functons, + where +
+ +{% highlight ruby %} +predition code +{% endhighlight %} diff --git a/_drafts/data-fusion.html b/_drafts/data-fusion.html new file mode 100644 index 0000000000..5bddb733ad --- /dev/null +++ b/_drafts/data-fusion.html @@ -0,0 +1,288 @@ +--- +layout: post +title: "Data Fusion" +subtitle: "What is the data fusion and how does it work" +date: 2022-02-10 +background: '/img/posts/kalman-filter/1.PNG' +--- + ++ Data fusion is process used to combine mulitple sensor reading to get more accurate + reading. For example, we have a robot move forward and backword. The robot use ladir + to measure the obestical and estimate the distance. All sensor they have a noise in their reading + and accuracy ( foe example, the ladir the robot use has a std of 2 cm therefore, if the robot read the distance + 20cm we can say the robot position fall between 18cm and 22cm). The noise in the sensor can be experse as gussain noise. + Going back to our robot, if the ladir show the wall is 25cm away from the robot with std 5m as shown in the figure below the green curve. +
+ +
+
++ We have other reading came from the odometry using encoder and it shows the robot position is at 7m with std 7m. + If we keep reading both sensor independently, the error in ladiar will stay same while the odometery will increase to the point + that cannot be use as reliable. Therfore we use data fusion to combine both reading to get a more accurate reading value. The + figure below shows the output of combining both reading. As can be seen that blue curve shows the position at 6m with a std 3m + which is much better than each sensor works independently. +
+ +
+
+
++ The process occurs at the top consisting of two steps which are (1) prediction step and (2) update step. + In the first step, the dynamic of the robot updates the current state \(x_t\) where the uncertainty grows while + the second step updates the current state \(x_t\) and reduces the uncertainty in the system. +
++ The fusion process can be done by Bayes' Theorem using different probability distributions or we can use the Kalman filter. +
++ The Kalman filter considers a robust mathematical model used in data fusion where it simplifies the problem. + The Kalman filter solve linear systems \( \hat x = f(x | z_1, .... z_n )\), where \(\hat x\) is the estimated state, \(x\) + is the uncertain state and \(z_1 , z_n \) is the noisy measured state. So the Kalman filter minimise the state using + the noizy measurment to get as close as possible to the accurate state \(\hat x = min J\). Therefore, the problem convert + to minimizing the error between the noizy state and the estimated state \(\overline x = x - \hat x \) , + where \(\overline x \) is the state estimation error. We have the state estimation Covariance \(P = [(x- \hat x)(x- \hat x)^T]\) + and the cost function Jacobian \( J= [(x- \hat x)^T(x- \hat x)] = Tr(P) \). + Therefore, we can minimise the cost function \(J\). +
+ ++ There are differently typed Kalman filters (KF) due to the computation efficiency. + However, due to the Kalman filter process, only linear equations and Extended Kalman filter + (EKF) was designed to work on a non-linear model by approximating the model to linearize the + relationship and use a linear Covariance. + Therefore, EKF works with models that can linearize. + Since some models cannot approximate a linear model, an Unscented Kalman Filter (UKF) + developed to work with non-linear models to calculate the prediction and updated equations. + The UKF linearize the uncertainty Covariance instead of linearizing the models' dynamics. +
+ ++ Now I am going to implement KF in Arduino to provide me with a more accurate data reading of + IMU sensor (MUP-6050). We will correct one roll angle of the sensor using Kalman gain. + Kalman gain uses to update the measure state in order to correct the new estimate state. +
+$$ + K = \frac {P*H} {H*P*H+R} +$$ ++ where \(P\) is error covariance, \(H\) measure map scalar, \(Q\) initial estimation covariance, and + \(R\) is noise coverance. The Kalman gain should be between 0 and 1, which refer to the stabiltity of the system, + 0 is stable estimate and 1 is unstable estimate. +
++ The class below is the Kalman filter algorithms for a singel input state. + The class define multiple virables which define in the function to compute the kalman gain. + The update function is used to compute the updated kalman gain and the corrected state. +
+ + +{% highlight ruby %} +class Kalamn_filter +{ + public: + const float R = 20; // noise coverance + const float H = 1.00; // measure map scalar + float Q = 2; // initial estimation covariance + float P = 0; //error covariance + float x_hat = 0; // updated state + float K = 0; //kalman gain; + float Update(int x) + { + + K = P*H/(H*P*H+R); // update kalman gain + x_hat = x_hat + K*(x-H*x_hat); + + // update error covariance + P= (1-K*H)*P+Q; + + return x_hat; + } +}; +}; +{% endhighlight %} + ++ The blow code is the setup and main function loop where we define the Serial and we initialize the sensor + and the kalman filter class. In the main loop we get the data from the snesor and pass it to the update function + to calculate the estimated state. +
+ +{% highlight ruby %} +Kalamn_filter kalman_filter(); +void setup() { + // put your setup code here, to run once: + Serial.begin(9600); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + // Initilize Kalman filter + } + + void loop() { + // put your main code here, to run repeatedly: + // Read normalized values + Vector normAccel = mpu.readNormalizeAccel(); + + // Calculate Roll + int roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI; + + float roll_correct = kalman_filter(roll); + // Output + Serial.print(" Roll = "); + Serial.print(roll); + Serial.print(" Roll_correct = "); + Serial.print(roll_correct); + + Serial.println(); + + delay(10); + } +{% endhighlight %} + ++ The figure below shows the raw data (green) and the output of the Kalman filter (gray). + As can be seen that the estimated roll has a smooth curve and less noize comparing to the measured roll angle. +
+ +
+
++ The complete code is below +
+ + + +{% highlight ruby %} +#include+ + +
+ ++ In this example I will use MPU6050 sensor that provide an acceleration data and gyroscop data. Using the acceleration data to compute + the state \(x\) and the gyroscop data to correct the state. +
+ + $$ + \theta = \theta_0 + \dot \theta t + \frac 1 2 \ddot \theta t^2 + $$ ++ So the state will be \(\hat x = [\theta { , } \dot \theta]^T \). where decribe the angle and the anguar velocity. + These describe our best estimation (\(\mu)\). Now the covariance will be + \( P_k = \begin{pmatrix} \Sigma_{\theta \theta} && \Sigma_{\theta \dot \theta} \\\ \Sigma_{\dot \theta \theta} && \Sigma_{\dot \theta \dot \theta}\end{pmatrix} \). + And our A matrix is shown below. +
+ +$$ + A_k = \begin{pmatrix} 1 && \Delta T \\\ 0 && 1 \end{pmatrix} +$$ ++ Therfore, the result of multiplication of \(x\) and \(A\) is a matrix (2x1) and shows below. +
+$$ + A_k x = \begin{pmatrix} \theta + \Delta T \dot \theta \\\ \dot \theta \end{pmatrix} +$$ ++ now on the control pat which is the angular acceleration where \(u=[\ddot \theta]\), and + \(B = \begin{pmatrix} \frac 1 2 \Delta t^2 \\\ \Delta t \end{pmatrix} \) and + the result of the multiplication between \(u\) and \(B\) is +
+ +$$ + Bx = \begin{pmatrix} \ddot \theta \frac 1 2 \Delta t^2 \\\ \ddot \theta \Delta t \end{pmatrix} +$$ + ++ Now we are going to update the state station using actual measurments \(Y_k = CX_k + z_k\), where + \(C=\begin{pmatrix} 1 && 0 \\\ 0 && 1 \end{pmatrix}\) and \(C=\begin{pmatrix} \theta \\\ \dot \theta \end{pmatrix}\) +
+ ++ The below code is implemented in arduino using MPU6050 sensors and it is based on the work of + Lauszus . + Where I modified to be used in class form. +
+ ++ The full code shows below +
+ + ++ The figure blow shows the output of the code as can be seen the estimation row angle is getting much better and smoother. +
\ No newline at end of file diff --git a/_includes/scripts.html b/_includes/scripts.html index 2af510e4cd..d67e90671c 100644 --- a/_includes/scripts.html +++ b/_includes/scripts.html @@ -82,3 +82,4 @@ }); {% endif %} + diff --git a/_layouts/default.html b/_layouts/default.html index 6fc9a457ee..e2ad1c227a 100644 --- a/_layouts/default.html +++ b/_layouts/default.html @@ -18,4 +18,9 @@