AVR Freaks

function uav() 

dt = 0.1; 
duration = 1000; 

measnoise = 0.1;
procnoise = 0.01;

a = [1 -dt; 0 1];               % transition matrix
b = [dt; 0];                    % input matrix 
c = [1 0];                      % measurement matrix 

x = [0; 0];                     % initial state vector      X = [ angle, bias ] 
x_est = x;                      % initial estimation equals initail state vector

Q = procnoise^2 * [dt^2 0; 0 0];% process covariance noise
P = Q;                          % initial estimation covariance
R = 0.1;                        % measurment covariance noise {R=0 Perfect follow} 

x_axis = 0:dt:duration; 
n = 1; 

mag = []; 

for t = 0 : dt: duration 
    input(n) = sin(x_axis(n)); 

    % Process modell
    za_noise(n)= input(n) + (procnoise * randn); 
    x = a*x + b*za_noise(n);
    
    % Measurement modell
    zg_noise(n)= (measnoise * randn); 
    y =  c*x + zg_noise(n); 

    % Innovation
    Inn = y - c*x_est;

    % Covariance of innovation
    S = c*P*c' + R;              
    
    % optimal Kalman gain
    K = a*P*c'*inv(S);
    
    % new State estimation
    x_est = x_est + K*Inn;
    
    % Covariance of prdiction error
    P = a*P*a' + Q;
    P = P - a*P*c'*inv(S)*c*P*a';
    
    mag=[mag;x_est(1)]; 
    n=n + 1; 
end 

plot(x_axis, input,'b', x_axis, mag,'r');

function uav() 

dt = 0.1; 
duration = 60; 

measnoise = 0.0001;
procnoise = 0.05;

a = [1 -dt; 0 1];               % transition matrix
b = [dt; 0];                    % input matrix 
c = [1 0];                      % measurement matrix 

x = [0; 0];                     % initial state vector      X = [ angle, bias ] 
x_est = x;                      % initial estimation equals initail state vector

Q = procnoise^2 * [dt^2 0; 0 0];% process covariance noise
P = Q;                          % initial estimation covariance
R = measnoise^2;                % measurment covariance noise {R=0 Perfect follow} 

x_axis = 0:dt:duration; 
n = 1; 

mag = []; 

for t = 0 : dt: duration 
    input(n) = sin(x_axis(n)); 

    % Process
    za_noise(n)= input(n) + (procnoise * randn); 
    
    % Time estimation    
    x = a*x_est + b*za_noise(n);
    
    % Measurement estimation
    zg_noise(n)=(measnoise * randn); 
    y = c*x + zg_noise(n); 

    % Innovation
    Inn = y - c*x_est;

    % Covariance of innovation
    S = c*P*c' + R;              
    
    % optimal Kalman gain
    K = a*P*c'*inv(S);
    
    % new State estimation
    x_est = x_est + K*Inn;
    
    % Covariance of prdiction error
    P = a*P*a' + Q;
    P = P - a*P*c'*inv(S)*c*P*a';
    
    mag=[mag;x_est(1)]; 
    n=n + 1; 
end 

plot(x_axis, za_noise,'b', x_axis, mag,'r');

My plan is to build a UAV like this: http://www.mikrokopter.com/ucwiki/MikroKopter. for stabilizattion

 I assume the 3D matrix ops use a 4x4 matrix... any estimate of how many fp ops to run one pass of the filter? or run one matrix mult or add?

dT = 0.1;
duration = 60;
cycles = 5;

gyroBiasDot = 0.1;
angleNoise = 0.025;
gyroNoise = 0.7;

x = [0; 0];

a = [1 -dT; 0 1];
b = [dT; 0];
Q = [0 0; 0 gyroNoise];
R = angleNoise;
P = Q;
H = [1 0];
I = [1 0; 0 1];

anglePlaneLog = [];
gyroPlaneLog = []; 
angleRealLog = [];
gyroRealLog = [];
angleKalman = [];
biasKalman = [];

gyroBias = 0;
n = 1;
time = 0:dT:duration;

for t = 0:dT:duration
    time(n) = time(n) + 1;

    anglePlane = sin(cycles * (2 * pi * t) / duration);                     % real world plane model (with ideal sensors)
    gyroPlane = cos(cycles * (2 * pi * t) / duration);

    gyroBias = gyroBias  + (dT * gyroBiasDot);                              % sensor signals (noisy and bias), derived from real world plane model
    gyroReal = gyroPlane + gyroBias + (((2 * rand) - 1) * gyroNoise);
    angleReal = anglePlane + (((2 * rand) - 1) * angleNoise);
    
    x = a * x + b * gyroReal;                                               % kalman filter prediction part
    P = a * P * a' + Q;
                                                                            
    inn = angleReal - H * x;                                                % kalman filter update part
    S = H * P * H' + R;
    K = P * H' * inv(S);
    x = x + K * inn;
    P = (I - K * H) * P;
    
    anglePlaneLog = [anglePlaneLog; anglePlane];                            % data logging for plot
    gyroPlaneLog = [gyroPlaneLog; gyroPlane];

    gyroRealLog = [gyroRealLog; gyroReal];
    angleRealLog = [angleRealLog; angleReal];
    
    angleKalman = [angleKalman; x(1)];
    biasKalman = [biasKalman; x(2)];
    n = n + 1;    
end    

subplot(2,1,1), plot(time, gyroRealLog, 'b', time, angleRealLog, 'r', time, angleKalman, 'y', time, biasKalman, 'c');
subplot(2,1,2), plot(time, anglePlaneLog, 'g', time, gyroPlaneLog, 'c', time, angleKalman, 'b', time, biasKalman, 'r');

function [poshat] = kalman(time,r, v, p)

% time = time vector 
% r = distance vector input
% v = upper limit for process (velocity) noise [m/10msec]
% p = upper limit for position measurement noise [m]
i = 2;                %time index
td = (time(i) - time(i-1)); %generate time steps
                                                  
M = [1 0; 0 1];           % System model A matrix  
x = [0 0]';               % Initial state 
xhat = x;                 % Initial state estimate

C = [1 0];                % Observation model

%???
W = v * [td^2 td; td 1]; % Process noise covariance ???
N = p; % Measurement noise covariance 
       % N=0 Perfect follow

P = W;   % Initial estimate covariance

pos = [];     % Position
posmeas = []; % Measured position
poshat = [];  % Estimated position 


for t = 0 : td : max(time),
    
    x = [r(i) (xhat(1) - r(i))/td]'; %place and velocity
    
    w = v * [td; 1];     % Process noise
    x = M * x + w;       % Simulate the system
    
    n = p;               % Measurement noise
    z = C * x + n;       % Simulate the measurement
    
    y = z - C * xhat;   % Innovation of measurement residual  
    S = C * P * C' + N; % Innovation covariance
    K = M * P * C' * inv(S);  % Optimal Kalman gain
    P = M * P * M' + W - M * P * C' * inv(S)* C * P * M';  % Estimate covariance
    
    xhat = M*xhat + K * y;                % State estimate using the Kalman filter
    i=i+1;
    % Save actual, measured and estimated positions
    pos = [pos x(1)];
    posmeas = [posmeas z];
    poshat = [poshat xhat(1)];
end

% Plot the results
%t = 0 : td : max(time);
%plot(t,pos,'r',t,posmeas,'g',t,poshat,'b');
%legend('true position','predicted position', 'updated position');
%xlabel('t [10ms]');
%ylabel('Position [m]');
%grid;

function [poshat] = kalman(time,r, v, p)

% time = time vector 
% r = radius vector input
% v = upper limit for process (velocity) noise [m/10msec]
% p = upper limit for position measurement noise [m]
time = [0; time];                       %add a new zero row 
time(2) = 1;                            %inhibit divide by zero
t = 2;                                  %time index
td = abs(time(t) - time(t-1));                                                  
M = [1 0; 0 1];                             % System model  
x = [0 0]';                                 % Initial state 
xhat = x;                                   % Initial state estimate

C = [1 0];                                  % Observation model

W = v * [td^2 td; td 1];                  % Process noise covariance ???
N = p;                                    % Measurement noise covariance 
                                          % N=0 Perfect follow

P = W;                                      % Initial estimate covariance

pos = [];                                   % Position
posmeas = [];                               % Measured position
poshat = [];                                % Estimated position 


[col row] = size (time);

for t = 2 : 1 : col,
    td = (time(t) - time(t-1));
    
   
    x = [r(t-1) (xhat(1) - r(t-1))/td]';   %place and velocity
    
    w = v * [td; 1];                  % Process noise ???
    x = M * x + w;                     % Simulate the system
    
    n = p;                             % Measurement noise
    z = C * x + n;                     % Simulate the measurement
    
    y = z - C * xhat;                              % Innovation of measurement residual  
    S = C * P * C' + N;                            % Innovation covariance
    K = M * P * C' * inv(S);                       % Optimal Kalman gain
    P = M * P * M' + W - M * P * C' * inv(S)* C * P * M';  % Estimate covariance
    
    xhat = M*xhat + K * y;                % State estimate using the Kalman filter
   
    
    % Save actual, measured and estimated positions
    pos = [pos x(1)];
    posmeas = [posmeas z];
    poshat = [poshat xhat(1)];

end