Issue with High Estimation Error in 2nd and 3rd Rows of State Vector

5 views (last 30 days)
I'm using an extended state estimation approach in MATLAB to estimate a 4-dimensional state vector. The first row's estimation is accurate, but the second and third rows consistently show over 20% error. I've adjusted the process noise (Q) and measurement noise (R) covariances without success. Below is a snippet of my code:
A = [-1 0 2; 2 -3 0; -1 -2 -3];
B = [0;0;10];
C = [0 0 1];
x_initial = [1; 1; 1];
measurement_noise_std = 0.01;
tspan = [0 1000];
[t, true_state] = ode45(@(t, y) system_dynamics(t, y, A, B), tspan, x_initial);
function dydt = system_dynamics(t, y, A, B)
u = 1;
dydt = A * y + B * u;
end
num_steps = length(t);
measurements = true_state(:, 3) + randn(num_steps, 1) * measurement_noise_std;
Q = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 10^-7];
R = 0.01;
x_estimate = [1; 1; 1; 1];
P_estimate = eye(4);
x_estimated = zeros(4, num_steps);
x_estimated(:, 1) = x_estimate;
K = 0;
for k = 2:(num_steps)
A_h = [-1 0 2 0;
x_estimated(4, k-1) -3 0 x_estimated(1, k-1);
-1 -2 -3 0;
0 0 0 0];
B_h = [0 ; 0; 10; 0];
C_h = [0 0 1 0];
xhat_estimate = x_estimated(:, k-1);
P_initial = reshape(P_estimate, [], 4);
[Tx, xhat_predict] = ode45(@(Tx, xhat_predict) xhat_fun(Tx, xhat_predict), [t(k - 1), t(k)], x_estimated(:,k-1));
[Tp, P_predict_vec] = ode45(@(Tp, P_predict_vec) P_predict_fun(Tp, P_predict_vec, A_h, Q),[t(k - 1), t(k)], P_initial);
x_estimated(:, k) = xhat_predict(end, :)' + K * (measurements(k) - C_h * xhat_predict(end, :)');
P_predict = reshape(P_predict_vec(end, :), 4,4);
K = P_predict * C_h' / (C_h * P_predict * C_h' + R);
P_estimate = (eye(4) - K * C_h) * P_predict * (eye(4) - K * C_h)' + K * R * K';
end
function dx = xhat_fun(Tx, xhat_predict)
u = 1;
dx1 = (-1 * xhat_predict(1)) - (2 * xhat_predict(3));
dx2 = (xhat_predict(4) * xhat_predict(1)) - (3 * xhat_predict(2));
dx3 = (-1 * xhat_predict(1))- (2 * xhat_predict(2)) - (3 * xhat_predict(3)) + 10 * u;
dx4 = 0;
dx = [dx1;dx2;dx3;dx4];
end
function dP = P_predict_fun(Tp, P_predict_vec, A_h, Q)
P_predict = reshape(P_predict_vec, size(A_h));
dP = A_h * P_predict + P_predict * A_h' + Q;
dP = dP(:);
end
figure('Position', [100, 100, 800, 400]);
subplot(1, 1, 1);
plot(t, measurements, 'r', 'LineWidth', 1.5);
hold on;
plot(t, x_estimated(3, :), 'b', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('State x_3');
title('True and Estimated State x_3');
legend('Measurements', 'Estimated State x_3');
grid on;
subplot(2, 1, 2);
plot(t, x_estimated(4, :), 'b', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('State x_4');
title('Estimated State x_4');
legend('Estimated State x_4');
grid on;
y = mean(x_estimated(4,:))
disp(y)
I suspect it could be a problem with system observability or something else I'm missing. Any advice on improving the estimation accuracy for these rows?

Answers (1)

Shishir Reddy
Shishir Reddy on 2 Sep 2024
Hi Om Prakash
Upon reviewing the provided code, I see that there is an inconsistency in the number of states between the initial system dynamics and the state estimation process.
Initial System Dynamics:
State Transition Matrix – ‘A’ : 3 x 3
Control Input Matrix – ‘B’ : 3 x 1
Measurement Matrix – ‘C’ : 1 x 3
The dimensions of these matrices confirms that the original system is defined with three states.
Introduction of a Fourth State:
In the state estimation process, a fourth state is introduced. This is evident from the code segment where the state estimate ‘x_estimate’ is initialized and updated.
  • The state vector ‘x_estimate’ is initialized with four elements, and the estimation process attempts to predict and update four states.
  • The dynamics for the fourth state are defined as static (dx4 = 0;), indicating no actual dynamics are contributing to this state.
The estimation process might not accurately reflect the true system behaviour if it includes an unnecessary state.
Including a fourth state without proper integration into the system dynamics and measurement model can affect the system's observability, potentially making it impossible to accurately infer the system's complete state from the measurements.
Therefore, for any system, especially if additional states are introduced, the observability matrix should be calculated to ensure it has full rank, confirming that the system is fully observable.
It can be done as follows –
% Observability check
O = obsv(A, C);
rank_O = rank(O);
fprintf('Observability matrix rank: %d\n', rank_O);
if rank_O < size(A, 1)
warning('The system is not fully observable.');
end
For more information regarding observability, kindly refer the following documentation. https://www.mathworks.com/help/control/ref/statespacemodel.obsv.html
I hope this helps in resolving the issue.

Categories

Find more on Polynomials in Help Center and File Exchange

Products


Release

R2024a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!