1

I am trying to implement a Kalman Filter for estimating the state 'x' (displacement and velocity) of an oscillator. The code is below and should be simple to follow. 

clear; clc; close all;
% io = csvread('sim.csv');
% u = io(:, 1);
% y = io(:, 2);
% clear io;

% Estimation of state of a single degree-of-freedom oscillator using
%   the Kalman filter
% x[n + 1] = A x[n] + B u[n] + w[n]
% y[n] = C x[n] + D u[n] + v[n]

% Here, x[n] is 2 x 1, u[n] is 1 x 1
% A is 2 x 2, B is 2 x 1, C is 1 x 2, D is 1 x 1

%% Code begins here
N = 1000;
u = randn(N, 1); % Synthetic input
y = randn(N, 1); % Synthetic output

%% Definitions
dt = 0.005;    % Time step in seconds
T = 1.50;     % Oscillator period
zeta = 0.05;   % Damping ratio

sv0 = max(abs(u)) * dt;
sd0 = sv0 * dt;
Q = [sd0 ^ 2 0.0; 0.0 sv0 ^ 2]; % Prediction error covariance matrix
smeas = 0.001 * max(abs(u));
R = smeas ^ 2;                  % Measurement error (co)variance scalar

wn = 2. * pi / Ts;

c = 2.0 * zeta * wn;
k = wn ^ 2;

A = [0. 1.; -k -c];
Ad = expm(A * dt);
Bd = A \ (Ad - eye(2));
Bd = Bd(:, 2);
C = [-k -c];
D = -1.0;

%% State-space model and Kalman filter
sys = ss(Ad, Bd, C, D, dt, 'inputname', 'u', 'outputname', 'y');
[kest,L,P] = kalman(sys, Q, R, []);

Here is my problem. I get the error: 'In the "kalman(SYS,QN,RN,NN,...)" command, QN must be a real square matrix with at most 1 rows.'.

I thought that QN = Q = const and should be 2 x 2, but it is asking for a scalar. Perhaps I don't understand the difference between Q and QN in MATLAB's 'kalman' help description. Any insights?

Thanks.

Salmonstrikes
  • 737
  • 1
  • 6
  • 25
  • 1
    If your read the `doc kalman` you see that there is an additional matrix **G** involved with regard to w (for which Q is the covariance matrix). Unfortunately it fails to describe how to set it. Usually I implement the filter myself as I find it easier (see ` docsearch "Time-Varying Kalman Filter"` for usable code) – bdecaf Sep 11 '14 at 09:26
  • Thanks, but my observations depend on the control input too (i.e. the matrix D is non-zero in my case). – Salmonstrikes Sep 11 '14 at 23:33
  • Also, I'm a little worried about numerical stability and things going wrong, which I assume MATLAB's 'kalman' has bells and whistles for. – Salmonstrikes Sep 11 '14 at 23:47
  • 1
    Personally I would only worry about once you get into it. Anyway `kalman` is just a script - so `edit kalman` will show you all it's code. This might help find you how G and Q should look like. – bdecaf Sep 12 '14 at 08:01
  • Good point. I'll try doing that. – Salmonstrikes Sep 12 '14 at 21:21

1 Answers1

1

MATLAB is assuming the process noise is only one stochastic variable and not two like Q is representing.

So you have to add the G and H matrices to your sys like so:

G = eye(2);
H = [0,0];
sys = ss(Ad, [Bd, G], C, [D, H], dt, 'inputname', 'u', 'outputname', 'y');

Just as a reminder, using MATLAB's syntax:

x*=Ax+Bu+Gw

y=Cx+Du+Hw+v
Stephen Rauch
  • 47,830
  • 31
  • 106
  • 135
Tomás Arturo Herrera Castro
  • 185
  • 1
  • 15