A(I) = B error.

This is what the error says "In an assignment A(I) = B, the number of elements in B and I must be the same.

Error in KF (line 6)
Xhat(i) =zeros(3,1);

How do I solve this.

A = [0.7474 0.453 0.0735; -0.441 -0.06106 0.01205; -0.0723 -0.5735 -0.1334]; 
B = [0.0421 0.0735 0.01205]'; 
C = [20 9 1]; 
U = 0; H = 1;
z = -0.37727 + 0.1*randn(50,1)
Xhat(i) =zeros(3,1);
P = eye(3,3);
Q = 0.00001
R = 0.01
for(i=2:50)
    X_(i) = A*Xhat(i-1) + B*U;
    P = A*P*A' + Q;
    K = P*H'*inv(H*P*H' + Q)
    Xhat(i) = X_(i) + K*(z(i)-H*X_(i));
    P = (eye(1,1)-K*H)*P;
end
figure
plot([1:50],z,'rs')
hold on
plot([1:50], -0.37727*ones(1,50),'k')
plot([1:50], Xhat,'g')

Answer 1

[Star Strider]

That’s actually fairly straightforward. You’re assigning a (3x1) vector vector of zeros to a scaler Xhat(i). That won’t work. Either:

Xhat(i) = 0;                % Scalar Assignment

or:

Xhat(i,:) = zeroes(3,1);    % Vector Assignment

You later refer to a subscripted ‘Xhat’. so the second version is likely what you want.

[PVR]

Yeah I tried that and it gives me this error "Error using + Matrix dimensions must agree. Error in KF (line 13) X_(i) = A*Xhat(i-1) + B*U;" And I figured this is related to Xhat dimension.

[Star Strider]

Your current error is similar to your previous one. Since ‘B’ is a (1x3) vector as well, see if changing it to:

X_(i,:) = A*Xhat(i-1,:) + B*U;
      ... CODE ...
Xhat(i,:) = X_(i,:) + K*(z(i)-H*X_(i));

helps. (I think that’s all of them. Your ‘A’ matrix seems to be commented-out.)

[PVR]

B is a 3x1 matrix. Also A was not commented. It was a typo in this post.

[Star Strider]

It took me a few minutes to figure out that Xhat(:,i-1) is the correct way to reference it even though Xhat(i-1) is a column vector. (I’ve been working with MATLAB since 1994 when I was introduced to it in a modern control course and never encountered this problem before.) However, I usually use the form: y=(C*expm(A*t)*B+D)*U. It’s easier.

This works:

A = [0.7474 0.453 0.0735; -0.441 -0.06106 0.01205; -0.0723 -0.5735 -0.1334]; 
B = diag([0.0421 0.0735 0.01205]'); 
C = [20 9 1]; 
U = zeros(3,1); 
H = 1;
z = -0.37727 + 0.1*randn(50,1);
Xhat(:,1) =zeros(3,1);
P = eye(3,3);
Q = 0.00001;
R = 0.01;
for(i=2:50)
    Q2 = B*U;
    Q1 = A*Xhat(:,i-1);
    X_(:,i) = A*Xhat(:,i-1) + B*U;
    P = A*P*A' + Q;
    K = P*H'/(H*P*H' + Q);
    Xhat(:,i) = X_(:,i) + K*(z(i)-H*X_(:,i));
    P = (eye(1,1)-K*H)*P;
end
figure
plot([1:50],z,'rs')
hold on
plot([1:50], -0.37727*ones(1,50),'k')
plot([1:50], Xhat,'g')
hold off

You may have to experiment with it to get the result you want. All I know is that it runs successfully.