diff --git a/README.md b/README.md
index 4506e64..bdc3f4b 100644
--- a/README.md
+++ b/README.md
@@ -27,35 +27,45 @@ K = 0.01;
 R = 1;
 L = 0.5;
 
-A = [0 1 0
-    0 -b/J K/J
-    0 -K/L -R/L];
-B = [0 ; 0 ; 1/L];
-C = [1  0  0];
-D = [0];     
-delay = 0;
-
-%% Model
-motor_ss = ss(delay,A,B,C,D);
+% Matrix A
+A = [0 1  0  0; 
+    -K*R*L -(J+K+R*2) 0  1; 
+     0 0  0  1;
+     0 1 -(b+J) -5*(J+R*2)];
+  
+% Matrix B
+B = [0 0; 
+     1 0; 
+     0 0; 
+     0 1];
+  
+% Matrix C
+C = [0 1 0 0; 
+     0 0 1 0];
+ 
+%% Model 
+motor_ss = ss(0, A, B, C);
 
 %% Input and time
 t = linspace(0, 20, 1000);
-u = [linspace(5, -11, 100) linspace(7, 3, 100) linspace(-6, 9, 100) linspace(-7, 1, 100) linspace(2, 0, 100) linspace(6, -9, 100) linspace(4, 1, 100) linspace(0, 0, 100) linspace(10, 17, 100) linspace(-30, 0, 100)];
+u = [linspace(1, 11, 100) linspace(7, 3, 100) linspace(6, 9, 100) linspace(-7, -1, 100) linspace(2, -10, 100) linspace(6, -9, 100) linspace(4, 1, 100) linspace(0, 0, 100) linspace(10, 17, 100) linspace(-30, 0, 100)];
+u = [u;2*u]; % MIMO
 
 %% Simulation
 y = lsim(motor_ss, u, t);
 
-%% Add 5% noise
+%% Add 15% noise
 load v
-for i = 1:length(y)
-  noiseSigma = 0.05*y(i);
+for i = 1:length(y)-1
+  noiseSigma = 0.05*y(:, i);
   noise = noiseSigma*v(i); % v = noise, 1000 samples -1 to 1
-  y(i) = y(i) + noise;
+  y(:, i) = y(:, i) + noise;
 end
 
+
 %% Identification 
 regularization = 838;
-modelorder = 3;
+modelorder = 10;
 [sysd, K] = okid(u, y, t(2) - t(1), 0, regularization, modelorder);
 close
     
@@ -65,7 +75,7 @@ close
     
 %% Check
 plot(t, y, t, yt(:, 1:2:end))
-legend("Data", "Identified", 'location', 'northwest')
+legend("Data 1", "Data 2", "Identified 1", "Identified 2", 'location', 'northwest')
 grid on
 ```
 ![a](https://raw.githubusercontent.com/DanielMartensson/Mataveid/master/pictures/OKID_Result.png)
@@ -330,7 +340,7 @@ close
     
 %% Check
 plot(t, yt(1:2, 1:2:end), t, yf(1:2, :))
-legend("Data", "Identified", 'location', 'northwest')
+legend("Identified 1", "Identified 2", "Data 1", "Data 2", 'location', 'northwest')
 grid on
 ```
 
diff --git a/pictures/OKID_Result.png b/pictures/OKID_Result.png
index 806b624..abea089 100644
Binary files a/pictures/OKID_Result.png and b/pictures/OKID_Result.png differ
diff --git a/sourcecode/listOfFunctions b/sourcecode/listOfFunctions
new file mode 100644
index 0000000..a8eba2e
--- /dev/null
+++ b/sourcecode/listOfFunctions
@@ -0,0 +1,9 @@
+eradc.m
+filtfilt2.m
+idbode.m
+ocid.m
+okid.m
+rls.m
+spa.m
+ssfd.m
+updatemataveid.m
diff --git a/sourcecode/spa.m b/sourcecode/spa.m
index 75aca7a..191dd72 100644
--- a/sourcecode/spa.m
+++ b/sourcecode/spa.m
@@ -38,7 +38,7 @@
   
   plot(freq, Y)
   ylabel('Amplitude')
-  xlabel('Frequency [rad/s]')
+  xlabel('Frequency [Hz]')
   
   % Return values
   amp = 20*log10(Y);