matlab 卡尔曼滤波编码

Question

T = 0.2;
A = [1 T; 0 1];
B = [T^2 / 2 T];
H = [1 0];
G = [0 1]';
Q = 0.00005;
R = 0.006;
x1(1) = 0;
x2(1) = 0;
x1e(1) = 0;
x2e(1) = 0;
xest = [x1e(1) x2e(1)]';
x1p(1) = 0;
x2p(1) = 0;
PE = [R 0; 0 0];
PP = A * PE(1) * A' + Q;

for i= 1:25
    if i < 10
       u = 0.25;
    else
       u = 0;
    end
    x1(i+1) = x1(i) + T * x2(i) + (T^2 / 2) * u;
    x2(i+1) = x2(i) + T * u + sqrt(Q) * randn;
    y(i+1) = x1(i+1) + sqrt(R) * randn;

    PP = A * PE * A' + G * Q * G';
    K = PP * H' * inv(H * PP * H' + R);
    PE = [eye(2) - K * H] * PP;

    xpredict = A * xest + B * u;
    xest = xpredict + K * (y(i+1) -H * xpredict);
    x1e(i+1) = [1 0] * xest;
    x2e(i+1) = [0 1] * xest;
end

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

Error in rrrr (line 34) x1e(i+1) = [1 0] * xest;

how i can solve the error

Answer 1

xpredict must be a 2x1 vector. To solve it, you need to transpose B in line 5 ie, B = [T^2 / 2 T]', Since from Newton's laws of motion with constant velocity, we have