Asked by Lina Alhelo
on 18 May 2019

Hi all;

I am perfoming kalman filtering on signal data from two sources, the output expected is as below. intially i have error in code I am using as follwoing

Error:

(Unable to perform assignment because the left and right sides have a different number of elements.left and right sides have a different number of elements.

Error in (line 56)

Z_buffer(k+1) = Z;

I dont know how to fix it and how to divide the output for different states, each 200 the matrix prameters are changing as shown in figure above.

At each state transition, A matrix and noises appropriately are changing

estimates of x and P at the end of each state will carry over into the next state.

how I can write code for this and fix the error.

Thank you in advanced

% Set true trajectory

Nsamples=800;

t=0:Nsamples;

Vtrue = 0;

% Xtrue is a vector of true positions of the train

Xinitial = 0;

Xtrue= rand(2,Nsamples);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Motion equations %%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Previous state (initial guess): Our guess is that the train starts at 0 with velocity

% that equals to 50% of the real velocity

Xk_prev = [0;

0];

% Current state estimate

Xk=[];

% Motion equation: Xk = A*Xk_prev + Noise, that is Xk(n) = Xk(n-1) + Vk(n-1) * dt

% Of course, V is not measured, but it is estimated

% A represents the dynamics of the system: it is the motion equation

A = [0.5 0;

0 0.3];

% The error matrix (or the confidence matrix): P states whether we should

% give more weight to the new measurement or to the model estimate

sigma_model = 1;

% P = sigma^2*G*G';

P = [1 0;

0 1];

% Q is the process noise covariance. It represents the amount of

% uncertainty in the model. In our case, we arbitrarily assume that the model is perfect (no

% acceleration allowed for the train, or in other words - any acceleration is considered to be a noise)

Q = [1 0;

0 1]; % the kalman gain appeared

% H is the measurement matrix.

% We measure X, so M(1) = 1

% We do not measure V, so M(2)= 0

H = [1 1];

% R is the measurement noise covariance. Generally R and sigma_meas can

% vary between samples.

sigma_meas = 1; % 1 m/sec

E_y = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Kalman iteration %%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Buffers for later display

Xk_buffer = zeros(2,Nsamples+1);

Xk_buffer(:,1) = Xk_prev;

Z_buffer = zeros(2,Nsamples+1);

for k=1:800

% Z is the measurement vector. In our

% case, Z = TrueData + RandomGaussianNoise

Z = H*Xtrue(k+1)+E_y;

Z_buffer(k+1) = Z;

% Kalman iteration

P1 = A*P*A' + E_x;

S = H*P1*H' + E_y;

% K is Kalman gain. If K is large, more weight goes to the measurement.

% If K is low, more weight goes to the model prediction.

K = P1*H'/(S);

P = P1 - K*H*P1;

Xk = A*Xk_prev + K*(Z-H*A*Xk_prev);

Xk_buffer(:,k+1) = Xk;

% For the next iteration

Xk_prev = Xk;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Plot resulting graphs %%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Position analysis %%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure;

plot(t,Xtrue,'g');

hold on;

plot(t,Z_buffer,'c');

plot(t,Xk_buffer(1,:),'m');

title('Position estimation results');

xlabel('Time (s)');

ylabel('Position (m)');

legend('True position','Measurements','Kalman estimated displacement')

Answer by Walter Roberson
on 18 May 2019

H = [1 1];

[...]

E_y = 1;

so H is a vector and E_y is a scalar

Z = H*Xtrue(k+1)+E_y;

Xtrue(k+1) is a scalar, multiply it by a vector H to get a vector, add the scalar E_y to get a vector, so Z is a vector.

Z_buffer(k+1) = Z;

Vector Z does not fit into the scalar location Z_buffer(k+1)

Sign in to comment.

Opportunities for recent engineering grads.

Apply Today
## 0 Comments

Sign in to comment.