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}=F{x}_k+w_k$$ $$y_k=H{x}_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_0{x}_k+\tilde{F}x+w$$ $$y_k=H{x}_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 ={\left[\begin{array}{ccccccc}\widetilde{F_{11}}& \cdots & \widetilde{F_{n1}}& \cdots \cdots & \widetilde{F_{1n}}& \cdots & \widetilde{F_{nn}}\end{array}\right]}^{\mathrm{\top }}$$

Then, let $\overline{x}={\left[\begin{array}{cc}{x}^{\mathrm{\top }}& {\theta }^{\mathrm{\top }}\end{array}\right]}^{\mathrm{\top }}$, formulate the system model as

$${\overline{x}}_{k+1}=G(\theta ){\overline{x}}_k+\left[\begin{array}{c}{w}_k\\ r_k\end{array}\right]$$ $$y_k=\left[\begin{array}{cc}H& 0\end{array}\right]{\overline{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 )=\left[\begin{array}{cc}{F}_0+\tilde{F}(\theta )& \\ & I\end{array}\right].$$

This model becomes nonlinear, so we choose EKF to estiamte $\overline{\mathbf{x}}$, and the jacobian matrix of $G(\theta )\overline{x}$ is

$$\frac{\partial G(\theta )\overline{x}}{\partial \overline{x}}(x_0,{\theta }_0)=\left[\begin{array}{cc}{F}_0+\tilde{F}({\theta }_0)& M(x_0)\\ 0& I\end{array}\right]$$

where

$$M(x)=\left[\begin{array}{cccc}{x}_1{I}_n& x_2{I}_n& \cdots & x_n{I}_n\end{array}\right].$$

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;