clear;
clc;
n_iter = 100;
t = 1:n_iter;
sz = [n_iter,1];
x = 24;
q = 4e-4;
r = 0.25;
t_start = 23.5;
delta_start = 1;
z = x + sqrt(r)*randn(sz); %100次的測量結果
這個**應該是別的地方拷貝的，具體**，我忘記了，以後找到了，會把鏈結加上。
clc
clear
close all
% 初始化引數
delta_t=0.1; %取樣時間
t=0:delta_t:5;
n = length(t); % 序列的長度
sz = [2,n]; % 訊號需開闢的記憶體空間大小 2行*n列 2:為狀態向量的維數n
g=10; %加速度值
x=1/2*g*t.^2; %實際真實位置
z = x + sqrt(10).*randn(1,n); % 測量時加入測量白雜訊
q =[0 0;0 9e-1]; %假設建立的模型 雜訊方差疊加在速度上 大小為n*n方陣 n=狀態向量的維數
r = 10; % 位置測量方差估計，可以改變它來看不同效果 m*m m=z(i)的維數
a=[1 delta_t;0 1]; % n*n
b=[1/2*delta_t^2;delta_t];
h=[1,0]; % m*n
n=size(q); %n為乙個1*2的向量 q為方陣
m=size(r);
% 分配空間
xhat=zeros(sz); % x的後驗估計
p=zeros(n); % 後驗方差估計 n*n
xhatminus=zeros(sz); % x的先驗估計
pminus=zeros(n); % n*n
k=zeros(n(1),m(1)); % kalman增益 n*m
i=eye(n);
% 估計的初始值都為預設的0，即p=[0 0;0 0],xhat=0
for k = 9:n %假設車子已經運動9個delta_t了，我們才開始估計
% 時間更新過程
xhatminus(:,k) = a*xhat(:,k-1)+b*g;
pminus= a*p*a'+q;
% 測量更新過程
k = pminus*h'*inv( h*pminus*h'+r );
xhat(:,k) = xhatminus(:,k)+k*(z(k)-h*xhatminus(:,k));
p = (i-k*h)*pminus;
end
figure
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'g-');
legend('含有雜訊的測量', '後驗估計', '真值');
卡爾曼 卡爾曼濾波 1
今天主要介紹一下卡爾曼濾波器，所謂卡爾曼濾波器其實是一種最優化遞迴數字處理演算法 optimal recursive data processing algorithm 卡爾曼濾波器應用 既然我們有了測量儀器，這些測量儀器可以目標給出準確測量值。還需要卡爾曼濾波器進行估計嗎?下面解釋一下為什麼需要卡...
卡爾曼 基礎卡爾曼濾波
卡爾曼濾波器是一種基礎 定位演算法。原理非常簡單易懂。核心過程可以用乙個圖說明 本質上就是這兩個狀態過程的迭代，來逐步的準確定位。更新 更具感測器獲取到比較準確的位置資訊後來更新當前的 問位置，也就是糾正 的錯誤。你可能要問為什麼有感測器的資料了還要進行更新？因為在現實世界中感測器是存在很多雜訊干擾...
卡爾曼濾波
卡爾曼濾波演算法 首先引入乙個離散控制過程的系統，用乙個線性隨機微分方程來描述 x k a x k 1 b u k w k 系統的測量值 z k h x k v k x k 是k時刻的系統狀態，u k 是k時刻對系統的控制量。a和b是系統引數，對於多模型系統，他們為矩陣。z k 是k時刻的測量值，h...