Документация

function [y] = kalman_filter(z,N0)
    %#codegen
    A = kalman_stm();
    
    % Measurement Matrix
    H = [1 0];
    
    % Process noise variance
    Q = 0;
    % Measurement noise variance
    R = N0 ;
    
    persistent x_est p_est
    if isempty(x_est)
        % Estimated state
        x_est = [0; 1];
        % Estimated error covariance
        p_est = N0 * eye(2, 2);
    end
        
    % Kalman algorithm
    % Predicted state and covariance
    x_prd = A * x_est;   
    p_prd = A * p_est * A' + Q;
    
    % Estimation
    S = H * p_prd' * H' + R;
    B = H * p_prd';
    klm_gain = matrix_solve(S,B)';
    
    % Estimated state and covariance
    x_est = x_prd + klm_gain * (z - H * x_prd);
    p_est = p_prd - klm_gain * H * p_prd;
    
    % Compute the estimated measurements
    y = H * x_est;    
end

function A = kalman_stm()    
    f0 = 200;
    dt = 1/1e4;
    % Kalman filter initialization
    % State transition Matrix
    A = [cos(2*pi*f0*dt), -sin(2*pi*f0*dt);
        sin(2*pi*f0*dt), cos(2*pi*f0*dt)];    
end

function x = matrix_solve(a,b)
    %fixed-point conversion friendly matrix solve: a * x = b
    
    % initialize x
    x = zeros(size(a,1),size(b,2));
    % compute lu decomposition of a
    [l, u] = lu_replacement(a);
    % solve x = a\b for every column of b 
    % through forward and backward substitution
    for col = 1:size(b,2)
        bcol = b(:,col);
        y = forward_substitute(l,bcol);
        x(:,col) = back_substitute(u,y);
    end
    
end

function [l,A]=lu_replacement(A)
    N=size(A,1);
    l = eye(N);
    for n=1:N-1
        piv = A(n,n);
        for k=n+1:N
            mult = divide_no_zero(A(k,n),piv);
            A(k,:) = -mult*A(n,:) + A(k,:);
            l(k,n) = mult;
        end
    end
end

function y = forward_substitute(l,b)
    % forward substitution
    N = size(b,1);
    y = zeros(N,1);
    % forward substitution
    y(1) = divide_no_zero(b(1),l(1,1));
    for n = 2:N
        acc = 0;
        for k = 1:n-1
            acc(:) = acc + y(k)*l(n,k);
        end
        y(n) = divide_no_zero((b(n)-acc),l(n,n));
    end
    
end

function x = back_substitute(u,y)
    % backwards substitution
    N = size(u,1);
    x = zeros(N,1);
    
    % backward substitution
    x(N) = divide_no_zero(y(N),u(N,N));
    
    for n = (N-1):(-1):(1)
        acc = 0;
        for k = n:(N)
            acc(:) = acc + x(k)*u(n,k);
        end
        x(n) = divide_no_zero((y(n) - acc),u(n,n));
    end
end

function y = divide_no_zero(num, den)
    % Divide and avoid division by zero
    if den == 0
        y = 0;
    else
        y = num/den;
    end
end

% KALMAN FILTER EXAMPLE TEST BENCH
clear all
step = ((400*pi)/1000)/10;
TIME_STEPS = 400;
X = 0:step:TIME_STEPS;
rng default;
rng(1);
Orig_Signal = sin(X);
Noisy_Signal = Orig_Signal + randn(size(X));
Clean_Signal = zeros(size(X));
for i = 1:length(X)  
Clean_Signal(i) = kalman_filter(Noisy_Signal(i), 1);
end
figure
subplot(5,1,1)
plot(X,rand(size(X)))
axis([1 TIME_STEPS 0 1.25]);
title('Noise')

% Plot Noisy Signal
subplot(5,1,2)
plot(X,Noisy_Signal)
axis([1 TIME_STEPS -4 4]);
title('Noisy Signal')

% Plot Filtered Clean Signal
subplot(5,1,3)
plot(X,Clean_Signal)
axis([1 TIME_STEPS -2 2]);
title('Filtered Signal')

% Plot Original Signal
subplot(5,1,4)
plot(X,Orig_Signal)
axis([1 TIME_STEPS -2 2]);
title('Original Signal')

% Plot Error
subplot(5,1,5)
plot(X, (Clean_Signal - Orig_Signal))
axis([1 TIME_STEPS -1 1]);
title('Error')
figure(gcf)