關於線性卡爾曼濾波的應用

大家晚上好，許久沒有更新博客，心血來潮寫一篇。今晚用力傳感器的時候發現對採集的信號直接作微分的信號像被狗啃過一樣。以前就瞭解過卡爾曼濾波，一直沒有機會用，今晚花了點時間看了幾篇csdn的博客然後實現了一下，推薦一下這個地址：
https://blog.csdn.net/revolver/article/details/37830675，裏面的視頻講得很良心。
沒時間排版，大家將就着看看。

數據下載

今晚在實驗室採集了一段傳感器的數據（emmm，有點失誤地令每個數據都重複了一遍，所以前面幾行matlab的代碼會有點奇怪），我自己寫了段代碼實現一個簡單的，只考慮了最簡單的$f=\dot{f}dt$這樣的模型。
數據可以在這裏下載：

鏈接：https://pan.baidu.com/s/16Yw_O4cudmhriKaLsUHzwg
提取碼：bjmj

狀態轉移方程及參數選擇

令：
$f(k)=f(k-1)+\dot{f}dt\\ \dot{f}(k)=\dot{f}(k-1)$
則有$X=[f,\dot{f}]$, $A =[1,dt;0,1]$, $H=[1,0]$, $U=0$;
$X(k) = AX(k-1)\\ Z(k)=HX(k-1)$
這樣形式的動力學狀態轉移方程會在高頻段部分出現一定的滯後，原因是我們認爲速度不變，不過沒辦法啦，真實的模型並不是線性的，就目前我的水平還辨識不了，所以這個模型的精度比較差，我令過程噪聲協方差矩陣$Q=[0,0;0,0.015]$。爲了知道測量噪聲協方差矩陣$R$的取值，我先取了0~3s的力，這部分測量值的真實值是一個常量（因爲我沒有往力傳感器加力），然後我取了它的方差，值在2.5e-4左右，比較小，因此我令$R=1e-3$.

實現公式

5條黃金公式如下：
$X(k|k-1)=AX(k-1|k-1)+BU(k) -------------(1)\\ P(k|k-1)=AP(k-1|k-1)A^T+Q--------------(2)\\ X(k|k)=X(k|k-1)+Kg(Z(k)-HX(k|k-1))---------(3)\\ Kg =P(k|k-1)*H^T(HP(k|k-1)H^T+R)^{-1}----------(4)\\ P(k|k) = (I-KgH)P(k|k-1)-----------------(5)$

實現結果

勉強可以用。

matlab代碼

forceTXT = load('forceSensorZ1.txt');
cut = 6000;
force = forceTXT(1:2:cut); HZ = 100;
dforce = [force(2:end); force(end)];
dforce = (dforce-force)*HZ;
t = (0:1:length(force)-1)/HZ; 
figure(11);
subplot(211);
plot(t, force,'LineWidth',1.5); xlim([0, 3]);
subplot(212);
plot(t,dforce,'LineWidth',1.5); xlim([0, 3]);

dt = 0.01; % 10ms
Xk1k1 = [0 0]';     % 迭代估計初始值
Pk1k1 = [0 0; 0 0]; % 不確定性初始值
Kg = 0; % 卡爾曼增益初始值 
% 估計力的狀態方程，B = 0; U = 0;
% Xkk1 = AXk1k1 + Q
% Zkk1 = HXkk1 + R
A = [1 dt;
     0 1 ];
Q = [0 0;
     0 0.015]; % 過程噪聲，影響的是狀態變量的協方差
H = [1 0];
R = 1e-3; % 1e-3
logx   = [];  % 測量值
logx_  = [];  % 估計值
logdx_ = [];  % 估計值
logt   = [];
for i=1:cut/2
    logt = [logt i*dt];
    Zk = force(i); % 當前時刻的測量值
    % 狀態方程
    Xkk1 = A*Xk1k1;        % (1)
    Pkk1 = A*Pk1k1*A' + Q; % (2) , 上一時刻的最優估計值和這一時刻的估計值之間的協方差
    % 結合觀測方程的估計
    Xkk  = Xkk1 + Kg*(Zk - H*Xkk1);  % （3） 最優估計值
    Kg   = Pkk1*H'/(H*Pkk1*H'+R);
    Pkk  = (eye(2)-Kg*H)*Pkk1;
    % 下一輪的迭代參數更新
    Xk1k1 = Xkk;
    Pk1k1 = Pkk;
    % 記錄信號
    logx  = [logx Zk];
    logx_ = [logx_ H*Xkk];
    logdx_ = [logdx_ Xkk(2)];
end
figure(34);
subplot(211);
plot(logt,logx); hold on;
plot(logt,logx_,'LineWidth',1.5); hold off;
subplot(212); 
plot(logt,dforce); hold on;
plot(logt,logdx_, 'LineWidth',1.5); hold off;

2019年10月25日 凌晨2：11 廣州天河