Kalman濾波器

考慮用如下狀態空間模型描述的動態系統$X(k+1)=\Phi X(k)+\Gamma W(k) \tag{1.1}$$Y(k)=HX(k)+V(k)\tag{1.2}$式中，$k$為離散時間，系統在時刻$k$的狀態為$X(k) \in R^n$；$Y(k)\in R^m$為對應狀態的觀測訊號；$W(k)\in R^m$為觀測噪聲。 在一個濾波週期內，從Kalman濾波在使用系統資訊和觀測資訊的先後次序來看，Kalman濾波具有兩個明顯的資訊更新過程：時間更新過程和觀測更新過程。

基於MATLAB下Kalman濾波的溫度測量模擬的實現

假設房間的溫度大概在25℃左右，我們以分鐘為單位，定時測量房間溫度，外界的天氣是多雲，陽光照射時有時無，有與外界空氣的交換，即引入噪聲$W(k)$，其方差為$Q$，大小假定為$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$

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');