现设线性时变系统的离散状态方程和观测方程

现设线性时变系统的离散状态方程和观测方程为：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

预估计X(k)^= F(k,k-1)·X(k-1)

计算预估计协方差矩阵

C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'

Q(k) = U(k)×U(k)'

计算卡尔曼增益矩阵

K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)

R(k) = N(k)×N(k)'

更新估计

X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]

计算更新后估计协防差矩阵

C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'

X(k+1) = X(k)~

C(k+1) = C(k)~

重复以上步骤

＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊

Ｍatlab实现代码

*********************************************************************************************************************************

%%%% Constant Velocity Model Kalman Filter Simulation %%%%

%==========================================================================

clear all; close all; clc;

%% Initial condition

ts = 1; % Sampling time

t = [0:ts:100];

T = length(t);

%% Initial state

x = [0 40 0 20]';

x_hat = [0 0 0 0]';

%% Process noise covariance

q = 5

Q = q*eye(2);

%% Measurement noise covariance

r = 5

R = r*eye(2);

%% Process and measurement noise

w = sqrt(Q)*randn(2,T); % Process noise

v = sqrt(R)*randn(2,T); % Measurement noise

%% Estimate error covariance initialization

p = 5;

P(:,:,1) = p*eye(4);

%==========================================================================

%% Continuous-time state space model

%{

x_dot(t) = Ax(t)+Bu(t)

z(t) = Cx(t)+Dn(t)

%}

A = [0 1 0 0;

0 0 0 0;

0 0 0 1;

0 0 0 0];

B = [0 0;

1 0;

0 0;

0 1];

C = [1 0 0 0;

0 0 1 0];

D = [1 0;

0 1];

%% Discrete-time state space model

%{

x(k+1) = Fx(k)+Gw(k)

z(k) = Hx(k)+Iv(k)

Continuous to discrete form by zoh

%}

sysc = ss(A,B,C,D);

sysd = c2d(sysc, ts, 'zoh');

[F G H I] = ssdata(sysd);

%% Practice state of target

for i = 1:T-1

x(:,i+1) = F*x(:,i);

%% Prediction phase

x_hat(:,i+1) = F*x_hat(:,i);

% State estimate predict

P(:,:,i+1) = F*P(:,:,i)*F'+G*Q*G';

% Tracking error covariance predict

P_predicted(:,:,i+1) = P(:,:,i+1);

%% Kalman gain

K = P(:,:,i+1)*H'*inv(H*P(:,:,i+1)*H'+R);

%% Updata step

x_hat(:,i+1) = x_hat(:,i+1)+K*(z(:,i+1)-H*x_hat(:,i+1)); % State estimate update

P(:,:,i+1) = P(:,:,i+1)-K*H*P(:,:,i+1); % Tracking error covariance update

P_updated(:,:,i+1) = P(:,:,i+1);

end

%==========================================================================

%% Estimate error

x_error = x-x_hat;

%% Graph 1 practical and tracking position

figure(1)

plot(x(1,:),x(3,:),'r');

hold on;

plot(x_hat(1,:),x_hat(3,:),'g.');

title('2D Target Position')

legend('Practical Position','Tracking Position')

xlabel('X axis [m]')

ylabel('Y axis [m]')

hold off;

%% Graph 2

figure(2)

plot(t,x(1,:)),grid on;

hold on;

plot(t,x_hat(1,:),'r'),grid on;

title('Practical and Tracking Position on X axis')

legend('Practical Position','Tracking Position')

xlabel('Time [sec]')

ylabel('Position [m]')

hold off;

%% Graph 3

figure(3)

plot(t,x_error(1,:)),grid on;

title('Position Error on X axis')

xlabel('Time [sec]')

ylabel('Position RMSE [m]')

hold off;

%% Graph 4

figure(4)

plot(t,x(2,:)),grid on;

hold on;

plot(t,x_hat(2,:),'r'),grid on;

title('Practical and Tracking Velocity on X axis')

legend('Practical Velocity','Tracking Velocity')

xlabel('Time [sec]')

ylabel('Velocity [m/sec]')

hold off;

%% Graph 5

figure(5)

plot(t,x_error(2,:)),grid on;

title('Velocity Error on X axis')

xlabel('Time [sec]')

ylabel('Velocity RMSE [m/sec]')

hold off;

%==========================================================================

*********************************************************************************************************************************

＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊