Kalman滤波器
考虑用如下状态空间模型描述的动态系统
X
(
k
+
1
)
=
Φ
X
(
k
)
+
Γ
W
(
k
)
(1.1)
X(k+1)=\Phi X(k)+\Gamma W(k) \tag{1.1}
X(k+1)=ΦX(k)+ΓW(k)(1.1)
Y
(
k
)
=
H
X
(
k
)
+
V
(
k
)
(1.2)
Y(k)=HX(k)+V(k)\tag{1.2}
Y(k)=HX(k)+V(k)(1.2)式中,
k
k
k为离散时间,系统在时刻
k
k
k的状态为
X
(
k
)
∈
R
n
X(k) \in R^n
X(k)∈Rn;
Y
(
k
)
∈
R
m
Y(k)\in R^m
Y(k)∈Rm为对应状态的观测信号;
W
(
k
)
∈
R
m
W(k)\in R^m
W(k)∈Rm为观测噪声。
在一个滤波周期内,从Kalman滤波在使用系统信息和观测信息的先后次序来看,Kalman滤波具有两个明显的信息更新过程:时间更新过程和观测更新过程。
基于MATLAB下Kalman滤波的温度测量仿真的实现
假设房间的温度大概在25℃左右,我们以分钟为单位,定时测量房间温度,外界的天气是多云,阳光照射时有时无,有与外界空气的交换,即引入噪声 W ( k ) W(k) W(k),其方差为 Q Q Q,大小假定为 Q = 0.01 Q=0.01 Q=0.01。对照(1.1)和(1.2)我们可以建立相应的状态方程和观测方程。
根据第k-1时刻的温度值来预测k时刻的实际温度
例如,在第k-1时刻,房间的温度真实值为24.0℃,测量值为23.9℃,偏差为0.1℃;在第k时刻,房间的温度真实值为24.1℃,测量值为24.5℃,偏差为0.4℃。
首先,利用k-1时刻温度值预计第k时刻的温度,预计偏差为
P
(
k
∣
k
−
1
)
=
P
(
k
−
1
)
+
Q
=
0.02
P(k|k-1)=P(k-1)+Q=0.02
P(k∣k−1)=P(k−1)+Q=0.02,计算Kalman增益
K
=
P
(
k
∣
k
−
1
)
P
(
k
∣
k
−
1
)
+
R
=
0.0741
K=\frac{P(k|k-1)}{P(k|k-1)+R}=0.0741
K=P(k∣k−1)+RP(k∣k−1)=0.0741,得到Kalman估计值
X
(
k
)
=
23.9
+
0.0741
×
(
24.1
−
23.9
)
=
23.915
X(k)=23.9+0.0741\times(24.1-23.9)=23.915
X(k)=23.9+0.0741×(24.1−23.9)=23.915。此时更新k时刻的偏差,可以对下一时刻观测数据进行更新处理。
Matlab R2016a 下的仿真结果
function main
N = 120;
CON = 25;
Xexpect=CON*ones(1, N);
X=zeros(1, N);
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1, N);
X(1)=25.1;
P(1)=0.01;
Z(1)=24.9;
Xkf(1)=Z(1);
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
F=1;
G=1;
H=1;
I=eye(1);
for k=2:N
X(k)=F*X(k-1)+G*W(k-1);
Z(k)=H*X(k)+V(k);
X_pre = F*Xkf(k-1);
P_pre=F*P(k-1)*F'+Q;
Kg=P_pre/(H*P_pre*H'+R);
e=Z(k)-H*X_pre;
Xkf(k)=X_pre+Kg*e;
P(k)=(I-Kg*H)*P_pre;
end
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for k=1:N
Err_Messure(k)=abs(Z(k)-X(k));
Err_Kalman(k)=abs(Xkf(k)-X(k));
end
t=1:N;
figure
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','Kalman 滤波值');
xlabel('采样时间/s');
ylabel('温度值摄氏/°C');
figure
plot(t, Err_Messure,'-b',t,Err_Kalman,'-k*');
legend('测量偏差','Kalman 滤波偏差');
xlabel('采样时间/s');
ylabel('温度值偏差/°C');