Inner matrix dimensions must agree. error

Hi everybody. I'm very news in matlab. I try to design kalman filter but the error occurred 'Inner matrix dimensions must agree' in matlab function

function [ x, P] = kalman( z, Q, R, x_old, P_old, A, H)

I = eye(3);

% Measurement update
K = (P_old * H') / (H*P_old*H'+R);
x = x_old + K * (z - H*x_old);
P = (I-K*H)*P_old;

% Time update
x = A * x ;
P = A * P * A' + B*Q*B';

Maybe initial estimates of P_old and x_old is not set I think. How to set that value? I want to set the first value for x and P are [0 ; 0 ;0 ] and [0.1 0 0; 0 0.1 0; 0 0 0.1] but dont know how to do.

[Walter Roberson]

Which line is the error occurring on? What are the size of each of the variables at that point? You might want to type in the command

dbstop if error

and then run your program; it will stop at the error location and allow you to examine values.

[Khoa]

x(3x1) , P(3x3), K(3x1), Q(3x3), R(1x1). From line 8 error occured (K = (P_old * H') / (H*P_old*H'+R);)

[Walter Roberson]

What size are P_old and H ?

Question: are you sure you want algebraic matrix division? Matrix inverse of one of the parts, matrix multiplied by the other part? Or are you wanting element-by-element division which uses the ./ operator instead of the / operator?

[Khoa]

P_old(3x3) and H(1x3). I upload my simulink below. Can u check it for me? http://www.mediafire.com/view/?7qla8km2q33jbs9 Thank you.

A = [1 5e-5 12.5e-10 ; 0 1 5e-5 ; 0 0 1];
H = [1 0 0];
Q = [0.1 0 0 ; 0 0.1 0 ; 0 0 0.1];
R = 0.1^2;
I = [1 0 0 ; 0 1 0 ; 0 0 1];

[Walter Roberson]

Please go in with the debugger, with dbstop if error, run until the problem occurs, and then try the subexpressions to see which part is generating the error

(P_old * H')
(H*P_old*H'+R)

and then if neither of those, finally

(P_old * H') / (H*P_old*H'+R)

With the array sizes you report, I do not reproduce the error.

I have to wonder about possibilities such as on the first iteration, something might be empty or an unexpected size.

Answer 1

[Azzi Abdelmalek]

As you said in your question, in your delay block you must set initials conditions to x0 and P0, where x0=zeros(n,1); n is your system order, and P0=0.1*eye(n). in your case n=3

[Khoa]

Thank you for your answer. But it still not work when I set initials conditions to x0 and P0 as you said. I post my simulink below: http://www.mediafire.com/?9wcwl6h1cwl6jx2 http://www.mediafire.com/?ycebwm7zly1tq88 I think the size of matrix x is not correct but not sure.

[Azzi Abdelmalek]

What is B in your code?, it should be H.

Corrected function

function [ x, P] = kalman( z, Q, R, x_old, P_old, A, H)
I = eye(3);
% Measurement update
K = P_old * H'/(H*P_old*H'+R);
x = x_old + K * (z - H*x_old);
P = (I-K*H)*P_old;
% Time update
x = A * x;
P = A * P * A' + H*Q*H';

Also, In the constant block H uncheck Interpret Vector as 1-D

Answer 2

[Khoa]

Sorry for reply late. I followed you and it worked correctly. But the output of kalman filter is not correct. My input of kalman filter is measurement position and I want to output position, velocity and acceleration. You can see it from scope1 and scope2(position input and velocity). I dont know how to fix kalman output. In other hand, My kalman block show bad result for output velocity and acceleration (scope5,6). I post my simulink below: http://www.mediafire.com/?l4mfgcupgmge4ij http://www.mediafire.com/?laa26ib5bgsco88