In the video you can see that the robot stays at roughly the same spot on the floor and handles a lighter push without any problem. To achieve this I have two cascade PID controllers and low pass filter on both wheel speed and the robot's angle.
I programmed a simple moving average filter. This will remove the rapid changes and make the robot calmer and with less overshoots. It also handles pushes much better than before. Video here.