卡尔曼经典公式:
状 态 一 步 预 测 : X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 状态一步预测: \qquad \hat{X}_{k/k-1} = \Phi_{k/k-1}\hat{X}_{k-1} 状态一步预测:X^k/k−1=Φk/k−1X^k−1
状 态 一 步 预 测 方 差 阵 : P k / k − 1 = Φ k / k − 1 P k − 1 Φ T + Γ k − 1 Q k − 1 Γ k − 1 T 状态一步预测方差阵: \qquad P_{k/k-1}= \Phi_{k/k-1}P_{k-1}\Phi^T+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1} 状态一步预测方差阵:Pk/k−1=Φk/k−1Pk−1ΦT+Γk−1Qk−1Γk−1T
滤 波 增 益 : K k = P X Z , k / k − 1 P Z Z , k / k − 1 − 1 或 者 写 为 : K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 滤波增益: \qquad K_k = P_{{XZ},k/k-1}P^{-1}_{ZZ,k/k-1}\\ 或者写为:\qquad K_k = P_{k/k-1}H^T_k(H_kP_{k/k-1}H^T_k+R_k)^{-1} 滤波增益:Kk=PXZ,k/k−1PZZ,k/k−1−1或者写为:Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1
状 态 估 计 : X ^ = X ^ k / k − 1 + K k ( Z k − H k X ^ k / k − 1 ) 状态估计:\qquad \hat{X} = \hat{X}_{k/k-1} + K_k(Z_k-H_k\hat{X}_{k/k-1}) 状态估计:X^=X^k/k−1+Kk(Zk−HkX^k/k−1)
状 态 估 计 方 差 阵 : P k = ( I − K k H k ) P k / k − 1 状态估计方差阵:\qquad P_k = (I- K_kH_k)P_{k/k-1} 状态估计方差阵:Pk=(I−KkHk)Pk/k−1
举例如下:
一维空间的卡尔曼滤波,主要参考严老师教材
X
k
=
0.95
⋅
X
k
−
1
+
W
k
−
1
y
k
=
X
k
+
V
k
X_k = 0.95 \cdot X_k-1+W_{k-1}\\ y_k = X_k + V_k
Xk=0.95⋅Xk−1+Wk−1yk=Xk+Vk
其中均值0,单位的高斯白噪声
clear;
% 系统参数
Phik = 0.95; Bk = 1.0;Hk = 1.0;
% 噪声参数,分别是标准差和方差
q =1;r = 3;Qk = q*q;Rk = r*r;
len =100;
%含有噪声的标准正太分布
w = q*randn(len,1);v = r*randn(len,1);
xk = zeros(len,1);yk = zeros(len,1);
xk(1) =r*randn(1,1);
for k = 2:len
xk(k) = Phik*xk(k-1) + Bk*w(k);
yk(k)= Hk*xk(k)+v(k);
end
% Kalman 滤波估计,初始状态为0
Xk = 0;
% 第一步的误差,这个数比较大,为很大的正实数???意义
Pxk = 100*Rk/(Hk^2*Phik^2);
for k=1:len
[Xk, Pxk, Kk] = kalman(Phik, Bk, Qk, Xk, Pxk, Hk, Rk, yk(k));
res(k,:) = [Xk,Pxk,Kk];
end
% 稳态滤波
ss = [Hk^2*Phik^2 Hk^2*Bk^2*Qk+Rk-Phik^2*Rk -Bk^2*Qk*Rk];
Px = ( - ss(2) + sqrt(ss(2)^2-4*ss(1)*ss(3)) ) / (2*ss(1));
K = Hk*(Phik^2*Px+Bk^2*Qk)/(Hk^2*(Phik^2*Px+Bk^2*Qk)+Rk);
G = (1-K*Hk)*Phik;
% 这里获取真值
Xk_IIR = filter(K, [1 -G], yk);
% 作图
subplot(1,2,1), hold off, plot(sqrt(res(:,2)),'-'), hold on, plot(res(:,3),'r:');
grid
xlabel('\itk'); ylabel('\it\surd P_x_k , K_k');
subplot(122), hold off, plot(yk,'x'),
hold on,
plot(xk,'m:');
plot(res(:,1),'k');
plot(Xk_IIR,'r-.');
grid
xlabel('\itk'); ylabel('\ity_k, x_k, x^\^_k, x^\^_k_,_I_I_R');
function [Xk, Pxk, Kk] = kalman(Phikk_1, Bk, Qk, Xk_1, Pxk_1, Hk, Rk, Yk)
Xkk_1 = Phikk_1*Xk_1;
Pxkk_1 = Phikk_1*Pxk_1*Phikk_1' + Bk*Qk*Bk';
Pxykk_1 = Pxkk_1*Hk';
Pykk_1 = Hk*Pxykk_1 + Rk;
Kk = Pxykk_1*Pykk_1^-1;
Xk = Xkk_1 + Kk*(Yk-Hk*Xkk_1);
Pxk = Pxkk_1 - Kk*Pykk_1*Kk';
end
结果图如下:
结果如下:
误差和滤波增益很快收敛,滤波曲线存在滞后的情况,数字滤波器的相位延迟特点。