1

I was reading this post and I have the same problem: I want to estimate the velocity using displacement and acceleration measures.

But the problem is that I don't know how I have to code/implement an algorithm using the matrices of Kalman filter presented in the post because there isn't equations with the matrices H, A, P, Q, R, ... And also the nomenclature from one post to other is different... It is the first time I need to use a Kalman filter, so I am not familiar with the use of this filter.

% data(:,1) Time
ts = data(2,1)-data(1,1);
ts2 = ts^2/2;
ts3 = ts^3/6;

nsamples = length(data);
x0 = mean(data(:,3));


ak = data(:,2)./(0.0991/9.81);       % Acceleration
dk = (data(:,3)-x0)./0.1266*2/1000;  % Displacement


% EXAMPLE MATRICES
HH = [1 0 0 0;
      0 0 1 0];

RR = [0.0100     0;
        0     0.0025];

AA = [1 ts ts2  ts3;
      0  1  ts  ts2;
      0  0   1   ts;
      0  0   0    1]; 

PP = [0 0 0   0;
      0 0 0   0;
      0 0 0   0;
      0 0 0 0.001];  

QQ = [0 0 0   0;
      0 0 0   0;
      0 0 0   0;
      0 0 0 0.001]; 


% Current state estimate
xk = [];
xk_prev = [dk(1); 0; ak(1); 0];

%        Disp           Velocity     Acc                   Jerk
xtrue = [ dk;  zeros(nsamples-1,1);   ak;   zeros(nsamples-1,1)];

% Buffers for later display
xk_buffer = zeros(4,nsamples);
xk_buffer(:,1) = xk_prev;
Z_buffer = zeros(1,nsamples);

for k = 1:nsamples-1 
    Z = xtrue(k+1);
    Z_buffer(k+1) = Z;

    P1 = AA*PP*AA' + QQ; 
    S = HH*P1*HH' + RR;

    K = P1*HH'/S; % inv(S)
    P = P1 - K*HH*P1;

    xk = AA*xk_prev + K*(Z-HH*AA*xk_prev);
    xk_buffer(:,k+1) = xk;

    % For the next iteration
    xk_prev = xk;    
end

(the language program used is MATLAB)

JMSH
  • 111
  • 3
  • 1
    Hi! Kalman filters are one of the most complex (both in theory and in practice) linear (and nonlinear with linearization) estimators out there. You better make some theoretical preparation for developing a sense of its principles and its internal behaviour. And you shall also see some working clear code examples. For these two purposes I believe an optimum entrance could be achieved through the book "Fundamentals of Kalman Filtering" from Paul Zarchan. Eventhough this site is not a book suggestion place, your possible state of DSP experience makes it a favorable answer though. – Fat32 Jul 11 '18 at 13:25

0 Answers0