Wednesday, April 15, 2015

A Kalman Filter for a Robot Car

This past week, I have spent quite a bit of time working on a simulator for a two-wheel differential drive robot.  The idea was to use simulated encoder and range finder data and an Extended Kalman filter to determine the location of a robot.  My implementation is written in Python and hosted here.

The system is modeled as having two inputs - The left and right wheel speeds.  The robot is externally given control inputs, but they are unknown to the Kalman filter.  The only inputs to the Kalman filter system are the encoder values and the range finder values and a set of known landmarks (this is to simulate mapping or some other landmark detection).  The encoder inputs are Vl and Vr.  From these values, we can surmise the average velocity V, and the rotational velocity w of the robot.
V = (Vl + Vr)/2
w = (Vr - Vl)/L, where L = length between wheels 
We can then use the image below to determine the simple version of the equations of motion of the robot.
An image of a differential drive robot.
From this system, we can surmise that the equations of motion, in discrete form, are:
x(k+1) = x(k) + V(k)*Cos(theta(k))*dt
y(k+1) = y(k) + V(k)*Sin(theta(k))*dt
theta(k+1) = theta(k)  + w(k)*dt
This means that we can assign the system states to be X, Y and Theta.  Because the system model above is ideal, we also need to add in a Gaussian noise value for each state. Now the system looks like this:
x(k+1) = x(k) + (V(k) + Nv) *Cos(theta(k))*dt
y(k+1) = y(k) + (V(k) + Nv) *Sin(theta(k))*dt
theta(k+1) = theta(k)  + (w(k) + Nw)*dt
The Kalman filter has two steps. The prediction step, and the update step.  The prediction step uses the system model, previous state values and control inputs (in this case, encoder values) to create a prediction (a priori) of the new states of the system.  It also creates an estimate covariance matrix, that is essentially a mathematical way to determine how much you trust the sensor values and the prediction.  The matrices needed for the calculation of the prediction step are the A, G, Q and P matrices.  A is the Jacobian, with respect to the system states, for the model shown above.  G is the Jacobian with respect to the noise values.  Q is a matrix built up by the covariances of the process noise.   The prediction step equations are:
X^(k+1) = f (X(k), U(k)) , where f (X(k), U(k)) is the original system model
P^(k+1) = A*P(k)*transpose(A) + G*Q*transpose(G)
The update step (a posteriori) is a bit more complicated.  The idea is to use a separate sensor (or multiple) to improve the quality of the prediction.  In this case, we will use a range finder to find landmarks and use those landmarks to determine where we should be and correct the position.  Of course, the range finder is not a perfect sensor and has Gaussian noise too.  This will be included in our calculations.  There are 4 parts of the update step.  The first is to calculate the difference in the measured values (from the range finder) and the predicted values from the previous step.  The h matrix, or measurement matrix, puts the states from the last step into the same space as the measurement values.  Then, we also need to calculate the Kalman gain.  This gain is a matrix that tells us how much we want to use the new measurement to update the prediction.  It is calculated using the sensor covariance matrix, the estimate covariance matrix and the Jacobian of the h matrix, H.  Using the residual and the Kalman Gain, we update the state prediction.  Finally, we updated the covariance matrix, using the Kalman Gain and H matrix.  The actual formulas are laid out below.
Y = Z - h(X(k))
K = P^(k+1)*H*(H*P^(k+1)*transpose(H) + R)^-1
X(k+1) = X^(k+1) + K*Y
P(k+1) = (I - K*H)*P^(k+1)
In order to make the landmark detection work, the x and y positions of the landmarks were augmented to the states matrix. X.  By adding those values, the Kalman Filter became an EKF and many other matrices had to be changed, as well.   The reason for doing this, is to keep track of the landmarks and allow them to make changes to the prediction, only if they are seen.  In my case, they are always seen, but in an real-world scenario, the range finder might not be able to see all of the landmarks or misses them.

Here are some results of my work.  A typical run can be seen below.  In this image, the actual position of the robot is in blue, the initial prediction is in green and the final prediction (after landmark detection with 20 landmarks) is in red.  The robot tracks the random path very well over 2 minutes, with a time step of 0.1 seconds.
The error between the actual and updated paths for this run can be seen below.  There was an average RMS error of 3.45 units over the course of the path.  What is neat, is that the Kalman Filter seems to correct itself when there is a lot of error. If odometry was the only sensor used, the error would grow much faster, and it would not correct itself.

Tuesday, April 7, 2015

The Lidar-Lite Ranging Sensor

Currently, I am working with the new PulsedLight Lidar-Lite range sensor, a small Laser emitter/receiver that can measure distance in one direction.  It is a terrific sensor that I bought for only $89 on-line.  A typical Lidar sensor is usually in the hundreds or even thousands of dollars, so this is quite a bargain, and something that the at-home maker can afford.  Not only is it cheap, it als has terrific accuracy and works with both PWM and I2C.

As of right now, I am writing a couple of libraries in Python and C++ to interface with the Lidar-Lite via I2C.  My work is currently posted here. Most of my experimentation has been using a Raspberry Pi with Rasbian Wheezy.  The idea is to make these libraries portable and then use them on a Robot Operating System (ROS) project, down the road.  Once the library is finished, I will post a quick tutorial on how to use it.   PusledLight also has a great repository of Arduino sketches for use the Lidar-Lite here.  I have tried them out and they work great.  Check it out!

Friday, March 13, 2015

3d Printer Reviews

One of the great perks of my job at Visa Labs is that I get to use 3d printers every day.  In my office, we have 4 printers that I have used extensively.  This post is a review of my experiences with these printers.

Flashforge Creator ($977)
5 Stars
The Flashforge Creator is the first printer we received and has proven to be the most reliable, fastest, easiest to repair and simplest to use printer of them all.  The Creator is one of the many printers to be based off of the original Markerbot Replicator.  It can use the same firmware as the Makerbots and even the same software (Makerbot Desktop, which works well).  It is also open-source, so it is very easy to find information, replace parts, and use open source software like Slic3r or replicatorG.  It can also print most materials with relative ease and had dual extruders which was great for multi-color prints.  The print quality at 0.2 mm layer height is fairly good for an inexpensive printer.

The only downside that I saw with the Creator was that there would often be lifting off of the build plate due to uneven heat on the surface.  Making large parts can be difficult because the build space is rather small and often there will be some problem like filament getting stuck or the material not sticking to itself.  Most of these problems are easy enough to fix with software or with creative solutions.  By far this printer exceeded my expectations and I would highly recommend it to beginners and advanced users alike.

Solidoodle 4 ($599)
4 Stars
Solidoodle 4
The Solidoodle 4 is another quality printer on the lower price range end of the spectrum.  I particularly liked the enclosed build area and overall build quality of the printer.  It was simple to fix any problems and had a great filament spool holder and feeding hole covered in rubber.  There were never any filament feeding problems at all, which is great because pretty much every other printer gets jammed all the time.  It also prints both ABS and PLA, which was good.

The downsides that I found with this printer included the software (Repetier Host) and the warped build plate.  First, the software was very odd compared to other software out there.  It gives you lots of control and can manually move the printhead which was nice.  However, it was non-intuitive to use and had alignment issues.  If you pressed the Home button multiple times, the printer would home each axis to different places.  It also caused the printer to not print the object in the same place that you wanted it to be printed.  This was frustrating, but even more so was the fact that the build plate that came with the printer was completely warped.   Because there are only three adjustments screws under the plate, it was not possible to manually un-warp the plate, so we could only print small objects in the area that was not warped.  Besides these small problems, we got very good quality prints at 0.2 mm layer height.

Makerbot Replicator 5th Generation ($2899)
3 Stars

MakerBot Replicator Desktop 3D Printer - 5th Generation
Makerbot is probably the most well-known 3d printer company around, but I was not so impressed with the 5th Generation Replicator.  There were a few positives that I found.  First, the Makerbot Desktop software is very good (although it takes some time to learn how to customize it).  It is very simple to use and can create an SD card x3g  g-code file for other printers as well.  I also liked the beautiful navigation screen and control wheel.  It made it easy to do things like align and level the build plate.  The build quality at 0.2 mm was mediocre and had a lot of bumpy edges, but certainly wasn't awful.

The negatives, however, were numerous.  First off, you could only print PLA because the build plate was not heated.  For an almost $3000 printer, that is pretty pathetic.  I also hated the new "Smart Extruder".  If by smart, they meant, gets jammed all the time, they did a great job.  Also, the blue tape they give you is absolutely useless.  From my experience, PLA does not stick to that stuff at all.  Also, the fact that the material is hidden inside the machine is awful, especially because the shape of the Makerbot spools are not the same as anyone elses.  If you want to use a 3rd party material, you have to make your own spool holder and hope that it works.  Lastly, I even had alignment issues with the onboard software.  The system thought the build area started in the middle and aligning it caused the print head to start bouncing off the wall repeatedly.  It was awful.  

Having said all of this, I would love to try out the Replicator 2, because I have heard much better things.  But consider me unimpressed by this Replicator.

CubeX Trio ($3295)
1 Star

The CubeX™ Trio 3D Printer
I have to say that I was most excited to try the CubeX Trio because of its specs.  Huge build space, three extruders, great motors, high accuracy and so on.  Boy, was I way off.  This thing is a piece of junk.  It seems like it is very well built, looks nice and has a seemingly great print head with fans.  And the two full prints that I managed to make, came out pretty well. These are the only nice things I can say about this machine.

Here are the problems I faced using this printer.  The software is horrible.  It is hard to use, has basically no ability customize anything and worst of all, you cannot connect it to the printer.  This means that you have to get everything right with the design and then put that design on the provided USB stick and plug it into the printer.  There is a growing group of people who have switched over to Kisslicer (open source) and use a post-processor to convert the g-code to the .brb format that the printer reads.  This was not simple to figure out and took me a very long time.  Kisslicer, I will say, is a good program with a lot of functionality.  The next problem is that the folks over at CubeX tried to do WAY too much with the onboard software.  The leveling and alignment process is all in software, so inevitably there are problems with mechanical heights versus software heights.  Now the company says that this printer can be used with PLA or ABS.  If you use ABS, you should use the included glue stick because the build plate is not heated.  Again, this is ridiculous for a three thousand dollar printer, but I digress.  This glue is like superglue, especially with the plastic build surface.  After finishing (or not finishing) any print, it would take me an hour or more to remove the print from the surface.  This is absurd and frankly intolerable.  But the worst part of this printer was the printing experience.  The printer would heat up, start extruding into the waste bin, then do about ten movements around the build area for some reason.  Then it would start to print, but it would print so slowly, that it was impossible to print fist sized object in less than 24 hours.  I tried to adjust the speed with Kisslicer, but to no avail.  Then came the extruder jamming problem.  It seemed that every single print I ever did with the CubeX, the extruder got jammed.  The worst part about the printer getting jammed was that the onboard software did not give you an option to pause the print and change the filament.  IT JUST CANCELLED THE PRINT!  So, again and again, I would waste hours on a seemingly good print only to have it fail in the middle and ruin my day.  There were countless more problems with this printer, but these were the most pronounced. I would recommend no one ever buy a CubeX printer.

Tuesday, March 3, 2015

The Wandboard and It's Mysteries

The Wandboard
For a while, at work, I was building up embedded systems for our team to use as development platforms.  One board, in particular was the Wandboard.  It is an ARM development platform built on the Freescale IMX6.  Although, I had a lot of trouble with this board, I was able to learn quite a bit about embedded OS's.

The Wandboard has a many choices of Operating Systems and the website features a few standard images.  My goal was to use Linux and our flavor of choice was usually Ubuntu.  Trying out the Ubuntu image, provided by the Wandboard people, we found that it was generally fine, but it ran graphics remarkably slowly.  Many other people had found the same thing.  Also, any updates to the system caused crashes.  This was no good.  So instead, I began looking for alternatives.

One alternative that I spent a very long time on was Robert Nelson's eewiki post about compiling Linux for the Wandboard from scratch.  This allows you more customization and you can learn about how an OS is built up.  After doing this procedure, many problems occurred, including no screen (which was fixed with uBoot args), update failures (from wrong repos), X11 failures (due to graphics drivers not being compatible after updates), and so on.  This was frustrating.

Next, I tried Yocto.  At first, I didn't quite understand what Yocto was, but after using it, I became familiar.  Yocto is a custom archlinux setup that is built on a development machine with all of the possible packages that you need for the system.  It is meant for production environments where the system setup just needs to be flashed to the device and never touched again.  This wasn't really what I was looking for, but I tried it anyway.  Boy, does it need some work,  First of all, building a correct system could take months.  Using bitbake (the graphical package selection tool), the system would fail after many packages.  The system is cool because it goes out and downloads the necessary files (based on package requirement files), compiles them for your particular system and then installs them into the image.  However, there are so many opportunities to fail, that it takes forever to get to the end of a setup.  I was able to finish a few setups and they worked just OK.  In the future, I would try to plan out exactly what packages and dependencies I need and go from there.

Lastly, I tried some other images from the Wandboard Wiki.  Again, these worked pretty well, but always, I ran into some problem with display, graphics and other things.  One big area of concern to me, is that I could never find an image that allowed the Wifi to work and the graphics acceleration to work at the same time.  There were images that had one or the other, or you could download the drivers for the wifi (BRCM29), but never both.  This just shows that Wandboard needs some serious work.

Later, I was able to get a UDOO Quad, which is essentially the same hardware, and I put one of their stock images on it and it worked perfectly.  Honestly, I was amazed that Wandboard's are so popular because they are a trainwreck to use.

Monday, April 7, 2014

Particle Swarm Optimization

Since my last post, I have spent a lot of time learning about biologically inspired optimization techniques.  Some commonly used biological machine learning methods include genetic algorithms and neural networks.  However, I decided to dive a little bit deeper and try out a few newer algorithms.  The two algorithms I have been working with are called Particle Swarm Optimization (PSO) and Ant Colony Optimization (ACO).  In this post I will talk about PSO.

Particle Swarm Optimization
This is a representation of Particle Swarm Optimization.  The solution that is found for a particle is swayed by the global best and the individual particle's best solutions.

PSO is a stochastic technique that is based on the way that swarms move towards a goal.  Each particle is a potential solution (or bee in a swarm) and it moves in some direction with a velocity.  The velocity is determined by a combination of the individual particle's best solution, the global swarm's best solution and an inertia weight.  First developed by Kennedy and Eberhart in 1995, each particle is updated with the equations shown below:

position and velocity equations
The interesting equation above is the velocity update.  Essentially, the new velocity for particle i is the addition of the previous velocity for that particle times the inertia weight (a function of how many iterations we have performed), the randomly weighted difference between the particle and the particle best and the randomly weighted difference between the particle and the global best.  randomly uniform weighting of the differences is a way of introducing unpredictability into the equation so that the system does not get stuck at a local minimum.  The position update just adds the new velocity to the previous position.

The algorithm for performing these actions is fairly straight-forward as well.  The idea is to update each particle and determine if that particle has a better fitness value than it has ever had before, and then determine if it has a better fitness than the swarm's best fitness.  This way, each particle has the equivalent of a unique mind as well as the swarm mind.  Below is the algorithm specifics:

  1. A set of N particles are created such that each particle contains random values in the set.
  2. It is determined whether each particle is its best case, if it is, particle i's best is this particle.
  3. It is determined whether the best particle is the swarms best case, if it is, the swarm best is this particle.
  4. Each particle's velocity and position are updated simultaneously with the new particle and swarm bests.
  5. If the global best meets the termination condition or the max iterations are reached, stop.  Else, go back to step 2 and repeat.
Using this algorithm, I was able to solve the N-queens problem.  My implementation can be found here, as a new Seeker in the Do-Anything-Machine.  In the N-queens problem, each particle is an array of rows containing a number that corresponds to a column for the position of each queen.  (For an explanation of the N-queens problem, see the previous post).  The fitness function is simply the number of queens that are on the same horizontals, verticals and diagonals of the chess board.  If the fitness of a particle is zero, the solution is correct.

One issue that I have noticed is that it can usually find a local minimum of the system, but does not always find a global minimum.  My solution was to repeat the algorithm multiple times, until the system met the fitness requirement.

Feel free to let me know what you think of my work in the comments.  I am happy to provide links and help to those who are interested.

Monday, March 24, 2014

The Do-Anything-Machine and the Hopfield Neural Network

Machine learning has become a new fascination for me in the past few months.  I have delved into the complicated world of solving problems with neural nets, genetic algorithms and clustering techniques.  Recently, I started work on the Do-Anything-Machine, a project started by Eric Neuman to build an artificial intelligence for solving any type of problem.  This is clearly a huge task, so it has been open-sourced.  To get involved, I have developed this method of solving the N-Queens problem.

The problem states that there are a number of ways to place N number of Queens onto a chess board with NxN squares, such that none of the queens can capture each other.  One way of solving this problem is by using a Hopfield Neural Network configuration.  A Hopfield net is a single-layer network in which all of the neurons are connected.  In our case, each neuron represents a square on the board, so there are N x N neurons.  Each neuron connection is weighted in a way that all of diagonal, horizontal and vertical squares from the initial square are added into that neuron, and the rest are weighted with zero.   The inputs are a random binary array, where each value in the array is an input into one of the neurons.  The output is also a binary array, with each value being an output from one of the neurons. 
This is an example of a Hopfield network with 6 neurons. (
The general method has two parts:
- The Internal process
- The External process

The internal process is an asynchronous method of changing the output of each neuron.
1. A random neuron is picked
2. The input potential of the neuron is calculated by summing the values of all of the horizontal, vertical, and diagonal queens from that square
3. This gives a value that is fed through a sigmoid function
4. The resulting output is then fed back as an input
5. The process repeats

The External process is performed after N x N iterations of the internal process.
1. The internal process is performed N x N times so that the inputs have a chance to evenly distribute
2. The energy (or cost function) of the entire board is calculated
3. If after 3 External iterations, the change in energy is zero, then the system has stabilized, and you have reached local minima
4. If it has not stabilized, then repeat the external process

Due to the amount of randomness, the system converges in anywhere from 3-25 external iterations.  The code for the initial implementation is here.  Please check it out and join in on the project!

Monday, August 12, 2013

Autonomous Quadricopter for Forest Fire Detection

I just recently finished my Master's Degree at Rochester Institute of Technology.  My final project was to design a control system for a Quadricopter that would be able to track forest fires as they spread.  All of the code is at  Here is the description from the paper:
"An autonomous control mechanism for tracking forest fires was designed with the use of an inexpensive, commercially available quadcopter.  The Parrot AR Drone 2.0 was chosen for this project because of its large open source programming community and many on-board sensors, including two cameras.  Image processing was performed on the video stream sent from the drone’s frontal, high definition video camera.  A few different fire detection algorithms were used with to make sense of the video data.  Then, after processing the video, object detection was performed to find the location of the fire.  Finally, the drone was told to move according to the location of the fire."
Below is the final presentation:

Wednesday, July 31, 2013

RIT Tigerbug Paper

I haven't written much about the Tigerbug project in a while, but I wanted to share the paper that our team wrote.  Please take a look and give any feedback that you have in the comments below.  I am very interested in what people have to say about the project!

Monday, July 22, 2013

Learning to Learn

I have always loved learning.  Sure, I haven't always made that point obvious, but in fact, it is true.  So as I depart the world of education for the excitement of the "real world", I can't help but reminisce about my glorious college days at Penn State and RIT.  But now, I am moving on; entering the unknown with nothing but the knowledge and skills I have acquired in 17 years of schooling.  From what I have heard from friends and read on the internet, I am as unprepared today as I was on the day I began school all those years ago.  But I disagree.  I have completed so many projects, homeworks and exams, spent countless hours researching, studying and taking notes. It has to count for something.  But what?  What do they count for?  I think the answer is that they have taught me how to work hard.  Sure everyone "works hard" once or twice in their life.  But at school, especially in my year at RIT, I have pushed myself to places I never thought I could go, almost daily.  I think my real education was in how to focus and complete a task to perfection.  At the end of the day, I am very proud of the work I have done.

Wednesday, July 17, 2013

The Sentience Stack

A good friend of mine, Eric Neuman, has proposed a roadmap to a living, motivated, creative, sentient being built entirely with Artificial Intelligence and computers.  The best part about the project is that it is open source (meaning github)!  The project is just beginning, but really seems to have legs.  Here is a more apt description of the project:

via simpleactually:
Artificial intelligence sounds awesome, doesn’t it? Machines that can solve problems on their own or answer questions for us would represent an enormous leap forward for all of mankind. The problem is, artificial intelligence as it exists today is not very flexible. Today, AIs are trading stocks, beating you at video games, filtering spam out of your email and handling a myriad of other tasks, each one specialized for its particular chore.  
Therein lies the rub, each AI needs to be custom built by a programmer specializing in artificial intelligence or machine learning.  In other words, it is currently possible to build machines that learn, but these machines can not learn how to learn.  If we want machines that can truly solve problems, answer questions on their own, and eventually grow to be sentient minds, we need to overcome that obstacle.  
I am joining in on this project and would encourage others interested in Artificial Intelligence to do the same.  Read more about the project at Eric's blog and get involved!