Adaptive State Estimation under Model Parameter Error
Problem Formulation
Model characterization of vehicle must contain errors in dynamical parameters. The parameter error cause inaccurate state estimation from sensors. I am using a simple example to present an idea of using Extended Kalman filter (EKF) to estimate states and parameter simultaneously to produce more accurate state estimation under parameter error.
$$x_{k+1} = Fx_k + w_k$$ $$y_k = Hx_k + v_k$$
where $w_k, v_k$ are the process and observation noise, $F \in \mathbb{R}^{n\times n},H \in \mathbb{R}^{m \times n}$ are state transfer matrix and observation matrix.
However, the $F$ matrix is unknown. We know an initial guess of $F$ as $F_0$. The difference could be denoted by $\tilde{F}=F-F_0$ which is also unknown. Thus, the system model becomes:
$$x_{k+1} = F_0x_k + \tilde{F}x + w$$ $$y_k = Hx_k + v$$
The problem is to estimate $x$ with the unknown $\tilde{F}$.
Simultaneous parameter and state estimation
Firstly, define a uncertainty vector by vecterizeing $\tilde{F}$:
$$ \theta = \begin{bmatrix} \tilde{F_{11}} & \cdots & \tilde{F_{n1}} & \cdots \cdots & \tilde{F_{1n}} & \cdots & \tilde{F_{nn}} \end{bmatrix}^{\top} $$
Then, let $\bar{x} = \begin{bmatrix} x^{\top} & \theta^{\top} \end{bmatrix}^{\top}$, formulate the system model as
$$\bar{x}_{k+1} = G(\theta) \bar{x}_k + \begin{bmatrix} w_k \\ r_k \end{bmatrix} $$ $$y_k = \begin{bmatrix} H & 0 \end{bmatrix}\bar{x}_k + v_k$$
where $r_k$ is a assumed noise term for the parameter error (can be chosen as a small value, would not affect filter’s performance), and
$$ G(\theta) = \begin{bmatrix}F_0+\tilde{F}(\theta) & \\ & I\end{bmatrix}. $$
This model becomes nonlinear, so we choose EKF to estiamte $\bar{\mathbf{x}}$, and the jacobian matrix of $G(\theta)\bar{x}$ is
$$ \frac{\partial G(\theta)\bar{x}}{\partial \bar{x}} (x_0, \theta_0) = \begin{bmatrix}F_0+\tilde{F}(\theta_0) & M(x_0) \\ 0 & I \end{bmatrix} $$
where
$$ M(x) = \begin{bmatrix} x_{1} I_{n} & x_{2} I_{n} & \cdots & x_{n} I_{n} \end{bmatrix}. $$
Results
Codes (Matlab):
%% Initialization
Total_time = 200; % total time
Interval_time = 2; % time interval
N = Total_time/Interval_time; % observation points
X_init = [0 0 0.2]'; % inital state
P_kalm = diag([8 10 5]); % initial covariance
V = 0.15; % observation noise
W = 0.05; % parameter nosie
% real state transfer matrix (unknown)
F = [1 2 2;
0 1 2;
0 0 1];
% initial guess of F (known from model characterization, contains error)
% F0 = [0.9 2 2;
% 0 1 2;
% 0 0 1];
F0 = F-0.1*rand(3,3);
H =[1 0 0]; % Observation matrix
[n_meas, n_state] = size(H); % dimensions
X_true_traj = zeros(n_state,N); % real state trajectory
X_true_traj(:,1) = X_init;
X_true_traj(3,:) = 0.2;
Z_meas_traj = zeros(n_meas,N); % real observation trajectory
Z_meas_traj(:,1) = 0;
X_pred_traj = zeros(n_state,N); % SE-KF prediction trajectory
X_pred_traj(:,1) = X_init;
X_kalm_traj = zeros(n_state,N); % SE-KF estimated trajectory
X_kalm_traj(:,1) = X_init;
X_aug_traj = zeros(n_state+n_state^2,N); % SPSE-EKF augmented state 轨迹
X_aug_traj(:,1) = [X_init;
zeros(n_state^2,1)];
%%
% ---------------------- 1. Real trajectory ----------------------
% using F
X_true_traj = zeros(n_state,N); % cache for real state trajectory
X_true_traj(:,1) = X_init;
X_true_traj(3,:) = 0.2;
Z_meas_traj = zeros(n_meas,N); % real observation trajectory
Z_meas_traj(:,1) = 0;
for i = 2 : N
X_true_traj(:,i) = F * X_true_traj(:,i-1); % state tranfer
Z_meas_traj(i) = H * X_true_traj(:,i) + sqrt(V) * randn; % observation
end
%%
% -------------------------2.State Estimation-KF (SE-KF)--------------------------
% using F0
for i = 2 : N
% prediction
X_pred_traj(:,i) = F0 * X_kalm_traj(:,i-1); % predict state
P_pred = F0 * P_kalm * F0'; % predict covariance
% Correction
K = P_pred * H' * inv(H * P_pred * H'+ V); % Kalman gain
X_kalm_traj(:,i) = X_pred_traj(:,i) + K * (Z_meas_traj(:,i) - H * X_pred_traj(:,i)); % update state
P_kalm = (eye(3)-K * H) * P_pred; % update covariance
end
%%
% ---------------------- 3. Simultaneous parameter and state estimation (SPSE-EKF) ----------------------
% using F0, and EKF
G = @(theta) blkdiag(F0+reshape(theta,n_state,n_state),eye(round(n_state^2))); % augmented state transfer matrix
M = @(x) [x(1)*eye(n_state), x(2)*eye(n_state), x(3)*eye(n_state)];
Jaco_x = @(x,theta) [F0+reshape(theta,n_state,n_state), M(x);
zeros(n_state^2,n_state), eye(n_state^2)]; % Jacobian matrix of the augmented state teansfer model
H_bar = [H zeros(1,n_state*n_state)]; % augmented measurement matrix
F_update = F0; % initial guess of F
P_ekf = blkdiag(diag([8 10 5]), zeros(n_state^2));
Q = blkdiag(zeros(n_state), W*eye(n_state^2)); % process noise for augmented state
meas_error = zeros(n_meas, N);
for i = 2 : N
% Prediction
theta = X_aug_traj(n_state+1:end,i-1); % get updated parameter at time i-1
X_aug_traj_pred = G(theta)*X_aug_traj(:,i-1); % model predict
P_pred = Jaco_x(X_aug_traj(1:n_state,i-1),theta) * P_ekf * Jaco_x(X_aug_traj(1:n_state,i-1),theta).' + Q; % predicted covariance
% Correction
K = P_pred * H_bar.' * inv( H_bar* P_pred * H_bar.' + V); % Kalman Gain
meas_error(:,i) = Z_meas_traj(:,i) - H_bar * X_aug_traj_pred;
X_aug_traj(:,i) = X_aug_traj_pred + K*(Z_meas_traj(:,i) - H_bar * X_aug_traj_pred); % obtain estimate at time i
P_ekf = (eye(n_state+n_state^2) - K*H_bar)*P_pred; % Update covariance
end
% ------------------------------4.plotting---------------------------------
% distance
figure(1);
plot((1:N),X_true_traj(1,:),'-black',(1:N),X_kalm_traj(1,:),'xg', (1:N),X_aug_traj(1,:),'ob')
title('Path Estimation');
xlabel('Time (s)');
ylabel('Distance (km)');
legend('Real trajectory','SE-KF trajectory','SPSE-EKF trajactory');
hold on;
% speed
figure(2);
plot((1:N),X_true_traj(2,:),'-black',(1:N),X_kalm_traj(2,:),'xg', (1:N),X_aug_traj(2,:),'ob')
title('Speed Estimation');
xlabel('Time (s)');
ylabel('Speed (km/s)');
legend('Real speed','SE-KF speed', 'SPSE-EKF speed');
hold on;
% error
figure(3)
plot((1:N),vecnorm(X_true_traj-X_kalm_traj,2,1),'g',(1:N),vecnorm(X_true_traj-X_aug_traj(1:3,:),2,1),'b');
title('Estimation Error')
xlabel('Time (s)')
ylabel('Error')
% ylim([0 5]);
legend('SE-KF error', 'SPSE-EKF error');
hold off;
