課上提到了KALMAN濾波器，稍微入個門，總算懂了點皮毛，總結了一下，如有錯誤歡迎指正
參考資料http://www.innovatia.com/software/papers/kalman.htm
不同於FIR、IIR經典頻域濾波器，KALMAN濾波器是時域濾波器，是通過時域上的包含噪聲的測量數據，計算出最接近實際值的方法
幾個關鍵公式如下
預測
1、假設
考慮在無摩擦無限長軌道上的一個物體靜止在0點
受到風的隨機擾動從而產生加速度ak （假設符合均值爲0，標準差爲σa的正態分佈），即過程噪聲。
每隔Δt時間測量一次位置，但該測量並不精確，存在隨機測量誤差，即測量噪聲。
現在我們用KALMAN濾波器來推測該物體的位置與速度
2、真實狀態
根據牛頓定律k時刻與k+1時刻有如下關係
化簡一下
![\textbf{x}_{k} = \textbf{F} \textbf{x}_{k-1} + \textbf{w}_{k}]()
- 其中
![\textbf{w}_{k} \sim N(0, \textbf{Q})]()
- 且
![\textbf{Q}=\textbf{G}\textbf{G}^{\text{T}}\sigma_a^2 =\begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix}\sigma_a^2.]()
3、測量
測量時存在噪聲誤差 vk ，且符合均值爲0標準差爲σz的正態分佈
其中
4、根據測量與估計判斷實際狀態
核心部分1、計算Innovation
用MATLAB仿真了一下
dt=0.1,duration=20,
measnoise = 10;
accelnoise = 1.5;
%http://blog.csdn.net/boksic
F = [1 dt; 0 1]; % transition matrix
H = [1 0]; % 測量矩陣
xk = [0; 0]; %初始狀態，位置速度都爲0
xhat = xk; % 初始估計值
Q = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = Q; % initial estimation covariance
R = measnoise^2; % measurement error covariance
% set up the size of the innovations vector
Inn = zeros(size(R));
pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array
Counter = 0;
for t = 0 : dt: duration,
Counter = Counter + 1;
%產生過程噪聲
ProcessNoise = accelnoise * randn * [(dt^2/2); dt];
%模擬產生真實狀態X
xk = F * xk + ProcessNoise;
%測量噪聲
MeasNoise = measnoise * randn;
%模擬測量結果
zk = H * xk + MeasNoise;
% 以下爲濾波
% Innovation，測量值與估計值的差
Inn = zk - H * xhat;
% Covariance of Innovation
s = H * P * H' + R;
% Gain matrix，可看做根據測量值與估計值的可信度得到的權重係數
K = a * P * H' * inv(s);
% 計算最優估計值
%F*xhat爲根據牛頓定律推算當前值
%K*Inn爲修正項
xhat = F * xhat + K * Inn;
% 更新標準差矩陣，從而不斷的循環
P = F * P * F' + Q - F * P * H' * inv(s) * H * P * F';
pos = [pos; xk(1)];
posmeas = [posmeas; zk];
poshat = [poshat; xhat(1)];
end
%顯示結果
t = 0 : dt : duration;
t = t';
plot(t,pos,'r',t,poshat,'g',t,posmeas,'b');
grid;
xlabel('時間 (sec)');
ylabel('位置 (feet)');
title('Kalman Filter Performance');
legend('真實值','預測值','測量值')運行結果
根據藍線的數值進行KALMAN濾波，噪聲濾除後的綠線數值與實際值紅線接近。