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

现设线性时变系统的离散状态方程和观测方程为： 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