Discrete algorithm for low pass filter

Question

I am working on a position controller for a marine vessel. I have a measurement signal containing the y-position of the vessel that consists of both low frequency (<.1 rad/s) and high frequency (>.6 rad/s) motions. I am currently using a state observer that separates that combined motion in two separate states (a LF and WF states). The gains are calculated using the discrete time Kalman filter algorithm.

At every timestep I receive a new incoming measurement and my state observer corrects the predicted state with the measurement.

I want to compare the performance of this approach with using a low pass filter on the measurement signal instead of using an Kalman filter. The theory seems to be that a Kalman filter has a much lower phase lag, but I am seeing some amplifications in the magnitude bode plot of my outgoing LF signal compared to the incoming measurements and the filter thus amplifies motions that are between the LF and the WF frequencies...

I need some advice on how to implement a nth-order low pass filter in such a discrete time system. Is there an algorithm to filter the incoming measurements at every time step, or do i need to take the entire time trace of the measurement and filter that signal? I am using Matlab.

Any tips are more than welcome, Scott

Answer 1

You can probably use Matlab's filter command. If you call it like this:

[Y,Zf] = filter(B,A,X,Zi);

the variable Zf contains the state of the filter after filtering, and Zi contains the filter's state before filtering. You could proceed roughly like this (in pseudocode):

Acquire several samples and store them in vector Samples
[Y,Z] = filter(B,1,Samples); % assuming FIR filter stored in B
while 1
    acquire new sample and store it in Sample
    [Y,Z] = filter(B,1,Sample,Z);  % Y contains the updated filter output
end

To design the filter, I'd start by looking at the fir1 command.