fungerer nesten i henhold til matlab script. gjenstår litt testing og finpuss