Kalman Filter 減低noise 的模擬試驗

使用時機：
當系統測量的過程，遇到隨機擾動的noise時，便無法真正測得真實的數據，為了減少擾動誤差，所以加入了卡爾門濾波器（Kalman Filter ）。

以純量，說明卡爾門濾波器運作原理：

prior X(k|k-1)=AX(k-1)+BU(k)..................(1)
prior P(k|k-1)=AP(k-1|k-1)A'+Q=P(k-1|k-1)+Q...............(2)
其中：
1.W(k)為第k時刻的process state noise，與X獨立，且滿足Probability Normal Distribution
p(w)~N(0,Q)
2.我們假設A=1(認定第k時刻和第k-1時刻無變異)
3.假設P(k)是X(k)的covariance： P(k|k-1)以k-1時刻推測k時刻的prior值

在第k時刻，同時會有一個measurement value Z(k)，與prior H(k)X(k)的Residual:
Residual =Z(k)-H(k)X(k|k-1)=Z(k)-X(k|k-1).................(3)
假設H=1(測量值與process state都是相同的單位，無物理轉換關係)

posteriori X(k|k)=X(k|k-1)+Kgain*(Residual).................(4)
1.其中Kgain是Kalman filter 增益
2.Kgain=P(k|k-1)H'(HP(k|k-1)H'+R)-1=P(k|k-1)/(P(k|k-1)+R), P(k)為(2)的值
3.R是measurement noise與Z獨立，且滿足Probability Normal Distribution
p(v)~N(0,R)
4.X(k|k)是後測的process state，推測第k時刻的最佳值

P(k|k)=(1-Kgain)*P(k|k-1)...................(5)

-------------------------------
程式：

clear all
clc
A=1;
B=1;
H=1;
U=0;
N=300;
desire=25*ones(1,N);

X=zeros(1,N);
measurement_noise=randn(1,N);
process_noise=randn(1,N);
Y=2^0.5*(measurement_noise)+desire;     %NormalDistribution random value

% set initial state variable and its covariance value
X(1)=0;
P=1;
Q=1e-4;   %set probability of process noise covariance P(w)~N(0,Q)
R=1e-1;  %set probability of measurement noise covariance P(v)~N(0,R)

for k=2:N
 X(k)=X(k-1);
 P=P+Q;
 KGain=P/(P+R);
 X(k)=X(k)+KGain*(Y(k)-X(k));
 P=(1-KGain)*P;
end
plot(X);
hold on
plot(desire,'r-');
plot(Y,'g*');
xlabel('time');
ylabel('temperature');
hold off

------------------------------------
[結果]





















其中
1.紅色水平線-期望值（用以觀察濾波後的值與之對照）
2.綠色點-未濾波時的數據（受到擾動很嚴重）
3.藍色線-濾波後的數據（很貼近期望值）