卡爾曼濾波（非矩陣）在平衡直立車直立角度測量中的應用（MATLAB模擬仿真），簡單易懂，詳細註釋。
前幾天學習了一下卡爾曼濾波算法，感覺濾波效果很好，然後就嘗試着運用卡爾曼濾波算法去解決一下實際問題，先從非矩陣的卡爾曼濾波算法開始，嘗試着將他應用到 平衡直立車直立角度測量中，對陀螺儀測得的角速度和加速劑測得的加速度進行角度融合，本次仿真中我們用設立的模擬量代替（根據實際情況，陀螺儀的噪聲干擾小，而加速度計的噪聲干擾大，所以在設定參數的時候，我們讓陀螺儀的值對最終結果影響大一些）
不多說了，解釋都放在註釋裏了，直接上代碼，註釋詳細，簡單易懂。
noise=randn(1,100); %產生1*100的標準正態分佈的隨機矩陣，用來模擬噪聲或者干擾
Z=15*ones(1,100)+10*noise; %模擬觀測量在數值15附近，波動幅值爲10，可以看出這是一個比較大的干擾或者噪聲
U=ones(1,100); %U用來模擬陀螺儀測得的角速度
Q=0.1; % Q，R ，X P是一些參數，根據實際情況修改
R=8; %根據實際情況，陀螺儀的噪聲干擾小，而加速度計的噪聲干擾大，所以在設定這些參數的時候，陀螺儀的值比重要大一些
X=0;
P=1;
figure;
hold on;
plot([1,100],[15,15],'g') %圖中的綠色線，即設定的系統穩定值，此處爲15，用來作參考
X_save=ones(1,100); %將用來保存每次迭代的結果
for i=1:100
X_=X+0.8*U(i); % X_爲角度的預測值，X爲上一次的角度，U爲陀螺儀測得的角速度，此處爲模擬值，物理上 X_=X+△t*U
P_=P+Q; % P_爲協方差預測值 Q表示預測模型的噪聲，初值P(0)的設定需要根據系統效果來調試確定，初值一般在1附近
K=P_/(P_+R); % K爲卡爾曼係數（用來影響我們相信預測值多一點，還是觀測值多一點），R爲觀測值噪聲，R越大，K越小，那麼觀測值對最終角度的影響就會變小，起到抑制噪聲的作用
X=X_+K*(Z(i)-X_); % Z(i)爲觀測值，即爲加速度計積分得到的角度，這個式子表示用觀測值來修正預測值
P=(1-K)*P_; % 修正協方差矩陣P，給下一輪迭代用
plot(i,X,'b.'); % 畫出每次迭代的結果
plot(i,Z(i),'r.'); %畫出每次迭代的觀測值（不經過卡爾曼濾波）
X_save(i)=X; %用來保存每次迭代的結果，方便繪圖
U(i+1)=15-X; %模擬經過電機調控後的角速度 （因爲是模擬，這個值是人爲設置的，實際應用上是由陀螺儀測得的）
end
j=(1:100); %畫圖的橫座標
% plot(j,X_save,'b');
% plot(j,Z,'r');
x_interp=linspace(0,100,500); %以下內容運用插值來畫出圖像，插值的目的是讓曲線更平滑，更好看，並不是必要的，在實際應用中是不需要的
%linspace函數生成一個數組，數據的第一個元素值爲0，
%最後一個元素爲100，500是總採樣點數，即在0到100之間取500個數，作爲插值點
y=spline(j,X_save,x_interp); %用spline函數對迭代結果進行三次樣條插值
y2=spline(j,Z,x_interp); %用spline函數對觀測值進行三次樣條插值
plot(x_interp,y,'b',x_interp,y2,'r'); %畫出插值後的曲線
運行結果：
示例一
示例二
示例三
由於數據是隨機產生的，所以程序的每次運行圖像都是不同的，圖中綠色的線表示我們設定的在沒有噪聲干擾下的穩定值，用來作參考，紅色的線是沒有經過卡爾曼濾波的在大的噪聲干擾下的觀測值，藍色的線是經過卡爾曼濾波後的值，經過對比我們可以發現卡爾曼濾波在大噪聲干擾下的濾波效果是很好的。
本文的主體部分到這就結束了，歡迎大家交流，互相學習，共同進步
附：卡爾曼濾波（矩陣）
以下內容參考neophack講的卡爾曼濾波器的原理以及在matlab中的實現，個人感覺他講的很好，簡單易懂，對於想學習卡爾曼濾波原理的同學可以去看一下
附鏈接：neophack講的卡爾曼濾波器的原理以及在matlab中的實現視頻鏈接
進行一次實驗，對100次迭代結果依次畫出：
out=VideoWriter('kaerman2.avi');
out.FrameRate=2;
open(out);
z=(1:100);
noise=randn(1,100);
z=z+noise;
x=[0;0];
p=[1 0;0 1];
f=[1 1;0 1];
q=[0.0001,0; 0 0.0001];
h=[1 0];
r=1;
for i=1:100
figure(i)
axis([0 110 0.5 1.5]);
axis manual
hold on;
plot([0,100],[1,1],'r')
x_=f*x;
p_=f*p*f'+q;
k=p_*h'/(h*p_*h'+r);
x=x_+k*(z(i)-h*x_);
p=(eye(2)-k*h)*p_;
plot(x(1),x(2),'b.','LineWidth',100);
F=getframe(gcf);
writeVideo(out,F);
close all
end
close(out);
結果視頻展示（進行一次實驗，對100次迭代結果依次畫出）：
kaerman2
附鏈接： 進行一次實驗，對100次迭代結果依次畫出視頻超鏈接
進行100次實驗，對每次實驗的結果依次畫出：
out=VideoWriter('kaerman.avi');
out.FrameRate=10;
open(out);
for n=1:100
z=(1:100);
noise=randn(1,100);
z=z+noise;
x=[0;0];
p=[1 0;0 1];
f=[1 1;0 1];
q=[0.0001,0; 0 0.0001];
h=[1 0];
r=1;
figure(n);
axis([0 110 0 2]);
hold on;
axis manual
plot([1,100],[1,1],'r')
for i=1:100
x_=f*x;
p_=f*p*f'+q;
k=p_*h'/(h*p_*h'+r);
x=x_+k*(z(i)-h*x_);
p=(eye(2)-k*h)*p_;
plot(x(1),x(2),'b.');
end
F=getframe(gcf);
writeVideo(out,F);
close all
end
close(out);
結果視頻展示（進行100次實驗，對每次實驗的結果依次畫出）：
kaerman