ELECINF344/381

Interactive web site of Télécom ParisTech's ELECINF344/ELECINF381 Robotics and Embedded Systems classes (a.k.a. ROSE, 2012 session).

Categories

BallE Kalman

As mentioned by Pierre-Hugues, we got the Kalman filter working today (C++ and OpenGL simulation). The Matlab implementation seems to work as well. The results are shown below. One tiny problem is left for the conversion from the estimated quaternion to the Euler angles (as we can see here for yaw: the plots have an offset…). This is not the case for the C++/OpenGL :)

Otilia Anton

A complementary filter for a IMU

I recently talked about getting informations from the FreeIMU board. This gives us raw data from acceleroscope and gyroscope. Both of them can give some informations about the attitude (pitch, roll) of the board, but they both have some flaws, even if they are perfect sensors (which they aren’t):

The gyroscope only gives the rotation speed, so the software has to integrates the value, which means there will most likely have integration errors, and thus a drift.
The accelerometer gives the acceleration + g, so when the device is not moving, we get g, which gives us pitch and roll. But when there is some acceleration, the values can’t be used.
The gyroscope’s drift is mostly slow, whereas there is an acceleration only on short period of times, so we can combine both of those sensors to get the actual attitude.

One usual way to merge them is to use a Kalman filter. It is a very effective way of merging, and the result can be quite incredible. But it is really complicated to understand and implement. The theory behind it assumes the knowledge of physics of the problem, which doesn’t help its complexity. It actually works without this knowledge, so it’s still a possible solution.

Anyway, as I mentioned before, the noise from the gyroscope and from the accelerometer is not a white noise, but has some specific spectrum, and gyroscope and accelerometer’s noises have quite a complementary spectrum, which leads to another possible filter to merge gyroscope and accelerometer informations (it can also filter magnetometer informations actually): It’s called a complementary filter.

The idea is to apply a low-pass filter on accelerometer, since its values are correct only at low frequencies, and add the complementary (ie the sum of the high-pass filter’s transfer function and the one of the low-pass filter is equal to one) on gyroscope’s data. That way, we filter out high frequency noise of the accelerometer, and low frequency noise of the gyroscope.

So, that’s the theory. To implement such a thing, an easy way we’ve chosen is to use an exponential filter: v_{n+1} = v_n*t + x_n*(1-t), where t is a number between 0 and 1. I’ve chosen t = 10/11 for a sensor frequency of 60Hz. The complementary filter is w_n = x_n-v_{n+1}.

Again, to have as simple code as possible, the current implementation doesn’t even use a FQA, which obviously leads to problems on some specific angles. For our bot, this is no problem, since the range of possible pitch and roll are short, we can take care of being in a safe area.

Today, we also got the Kalman filter to work, so we could compare both approaches. Even thoguh Kalman is still working better, the complementary filter is working properly, and should be enough to control the bot properly, at a way lower cpu processing cost.

Pierre-Hugues Husson

BallE Kalman

During the last two weeks, the Kalman filter for BallE has evolved. We now have the mathematical model as well as the Matlab implementation.  3 crucial questions were raised before reaching this point.

System and measurement equations:

x_k= F_(k-1)*x_(k-1) + G_(k-1)*u_(k-1) + w_(k-1)

y_k= H_k*x_k+v_k

where v_k and w_k are the measurements’ noise and the process’ noise respectively

X_k describes the state of the system at moment k, Y_k the measurements, F_k the transition matrix, H_k the measurement matrix, u_k the command (in our case we consider we don’t have any command applied for the time being)

Questions:

1) What type of Kalman filter should we use?

After a literature review and the study of the possible filters, I decided that the Sequential Kalman Filter (SKF) was the most appropriate for our problem: SKF does not require matrix inversion for computing the gain matrix (K). However, in order to use this type of filter we must be sure that the measurement covariance is a diagonal matrix.

2) Knowing that we obtain data from the accelerometer, gyroscope and magnetometer, how can we model the state? How should we deal with the gyroscope’s drift?

To answer this question, I first analyzed filters with 6DOF where the state is defined by X_k= [alpha bias]k (as presented  here: http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data ). However, as we have the magnetometer we can use only the accelerometer and magnetometer to compute the Euler angles (Y_k = [roll pitch yaw]k ). We can put as well the bias of the gyroscope in X_k but what exactly should define the state of the system?

Starting on this path, we searched more information about the approach used by Copterix last year (http://copterix.perso.rezel.net/?page_id=26). After studying “Airborne attitude estimation using a Kalman filter”, the Master thesis of Matthieu Marmion(1), Richard Murray’s lectures on Sensor Fusion and Factorized Quaternion Algorithm, we used the following model:

X_k=[bias quaternion]k

Y_k=[roll pitch yaw]k

F_k the same as the one presented in (1)

H_k the Jacobian of the Euler angles in fonction of the quaternion

3) Do we really need to compute H_k or can we find a simpler form for it?

We know the conversions for Euler to quaternion/ quaternion to Euler, therefore we can write directly the Euler angles in fonction of the X_k. However, we will further need H_k for the filter (for computing the gain matrix and the a posteriori state estimate and a posteriori state covariance). I computed the Jacobian and implemented the whole approach in Matlab.

After testing it with real data obtained from our IMU, the filter didn’t give satisfactory results…Once I finished my code and after calibration (measurement noise, initialization, time step) and after performing the tests I tried to understand where the problem might be. Yesterday I looked at the Copterix implementation and noticed that the set Y_k= [quaternion]. This simplifies enormously H_k. If we take Y_k=[quaternion]k we obtain H_k=[O4x3 I4]. However I wonder if the measurement noise covariance is still diagonal in this case.

Therefore, after today’s presentations I did the necessary changes to my code to work with H_k=[O4x3 I4]. The results are better but the filter doesn’t seem to work properly yet…

During the holiday I will do my best to find out what is the problem and to fix it. If you have any suggestions with what may be wrong in this approach please let me know:D

 

Otilia Anton

Pregnancy

Great news : Ball-E is due to be born soon !

Until Wednesday, I worked on the PCB with Pierre-Hugues and it has been sent in order to be produced (We even have pictures of the PCB’s birth). Spot welding might be our next week’s work !

As we have already received our stepping motors and omniwheels, till today I have been working on the architecture of the robot and it will soon be done, just take a look at the base of the robot ! I made it so that it is possible to change the angle of the omniwheels relatively to the ball.

Scott is working on the simulation part, and Otilia is working hard to make the Kalman filter work before Easter vacation.

 

Matthieu Tardivon

Ball-E news

First of all, I would like to apologize for not having given you news about Ball-E for such a long time.

Let me make a short summary of where we are now.

Before the Athens week, we decided several components of our robot and we received most of them. Actually, we have our 3 stepping motors (Trinamics QSH5718), our two-rows 4″ omniwheels (Kornylak). With Pierre-Hugues, we made the pin assignment of our STM32405RG-based card and finished the schematic of our PCB. Routing will be done as soon as Alexis will have corrected our schematic. Scott and Otilia are working hard in order to implement a Kalman filter and to understand a huge part of the mechanics.

Alexis lent us a card with an accelerometer, a gyroscope and a magnetometer so that Otilia and Scott can test their Kalman filter. Pierre-Hugues is going to code tomorrow a bootloader which will make the card dump the values of the sensors through the usb port.

My objective of tomorrow is to build the whole robot so that it will be ready for real tests when we will receive the PCB.

Matthieu

Matthieu Tardivon

BallE – mechanics and control review

Before the Athens week I started studying the Kalman filter. At the begining of this week I decided to work along with Scott who has been studying the mechanics of Rezero and BallIP as they were presented in the reports we found online. As two heads are better than one, since Monday we have been focusing on the mechanics, control and filters as a whole. The control system as well as the model are different for Rezero from the ones used by BallIP. Certainly Rezero performs better but BallIP seemed  more comprehensible at this step. In most of the papers that we read the authors first implemented what was previously achieved with ballbot technology and then built upon those concepts. Therefore today we focused on understanding the planar model (forces, energies, inertia calculations, torque conversion).

Certainly, in practice this model has limitations: the effect of the wheels is not decoupled – this highly influences the angle of tilt (before Rezero the tilt angle achieved was maximum 5 degrees whilst Rezero can recover from a 20 degree tilt). However, we considered we had more chances to understand the planar model before starting directly with the 3D model of Rezero. One of our goals today was to actually implement a Simulink simulation based on the description given in the Rezero report for the planar model.

We discovered that that there is a significant learning curve to designing a Simulink simulation model. We had a nice surprise stumbling upon a 2D model of an inverted pendulum through the Simulink Demos. Though it is not exactly what we need, we intend to use it as a basis for learning and simultaneoulsy adapting it to the planar model.

More on this tomorrow:)

For BallE – Otilia & Scott

 

P.S. on Monday I have studied the robots’ parameters (mass of the ball, mass of the body, radius of the ball, moment of inertia…). Rezero and BallIP are have significant mechanical differencies: the center of mass of Rezero is significantly higher than the center of mass of BallIP; the ball used for Rezero is a hollow aluminium one of 2.29kg coated with soft rubber whilst the ball of BallIP is a bowling 3.8kg ball coated with Plasti Dip. We could try Plasti Dip as well: http://www.plastidip.com/home_solutions/Plasti_Dip

Otilia Anton

Kalman, PCB and paper

We started the week by working on Ball-E. I am very happy with the protoype that Matthieu & PH built on Monday afternoon, it gave us an idea on what motors we need and we had the chance to check that the system works (the mechanical part). While baby Ball-E was being built, I have been digging for information on the Kalman filter applied to problems of inverted pendulum. Dan Simon’s “Optimal State Estimation” gave me a better understanding of the mathematical context. Apparently once we have our kinetic model we can implement the filter. I still need to clear up some thoughts on applying it to Ball-E (sensor fusion… in which order, what defines the state -acceleration, orientations, how will we process data from the IMU).

Meanwhile yesterday I finished my PCB. I was satisfied about the final placement. I hope the routing is good. Still there were some ground vias missing in the end and I had forgotten the command that would have enabled me to perform the final corrections.

Today I worked on the paper on firmware and bootloader. It was the chance to use Latex after a long pause. Writing required re-reading some of the papers we studied for the presentation and searching for new information. Brice and Jeremy worked on their parts as well and we discussed in the evening on the paper’s structure, content and what each one of us has written.

 

Otilia Anton