Add correction block

Convert correction calculation from matlab to omnet++