讀了詳解卡爾曼濾波原理之後,按照原文的思路做了**,分享一下我的**和結果。
在第50步的時候給系統狀態加了突變,但後驗誤差並沒有變大,抗干擾性能很好。
clc; clear all; close all;
% 引數
p=randn(1); % 狀態-位置
v=randn(1); % 狀態-速度
dt=1; % 時間間隔
f=[1 dt; 0 1]; % 狀態轉移矩陣
u=0.5; % 控制量
b=[dt*dt/2; dt]; % 控制矩陣
w=0.1; % 狀態干擾大小
q=w*eye(2); % 狀態干擾
h=eye(2); % 測量矩陣
r=0.1; % 測量誤差大小
r=r*eye(2); % 測量誤差
% 賦初值
t=100; % **長度
x=zeros(2,t); % 真實狀態
x(:,1)=[p;v]; % x初值
xpri=zeros(2,t); % 每時刻的先驗估計
ppri=q; % 先驗協方差初值非零
zk=zeros(2,t); % 測量值
zk(:,1)=h*x(:,1)+randn(1)*sqrt(r); % 測量值初值
xpos=zeros(2,t); % 每時刻的後驗估計
ppos=zeros(2,2,t); % 每時刻的後驗協方差
k=ppri*h'*inv(h*ppri*h'+r); % 卡爾曼增益
xpos(:,1)=xpri(:,1)+k*(zk(:,1)-h*xpri(:,1)); % 後驗估計變化
% 狀態變化
for k=2:t
x(:,k)=f*x(:,k-1)+b*u+sqrt(w)*randn(1); % 真實狀態變化
if k==50 % 狀態突變
x(:,k)=f*x(:,k-1)+b*u+10;
endxpri(:,k)=f*xpos(:,k-1)+b*u; % 先驗估計變化
ppri=f*ppri*f'+q; % 先驗協方差變化
zk(:,k)=h*x(:,k)+randn(1)*sqrt(r); % 測量值
k=ppri*h'*inv(h*ppri*h'+r); % 卡爾曼增益
xpos(:,k)=xpri(:,k)+k*(zk(:,k)-h*xpri(:,k)); % 後驗估計變化
endfigure(); hold on; grid on;
plot(1:100,xpri(1,:)-x(1,:),'g','linewidth',1);
plot(1:100,zk(1,:)-x(1,:),'b','linewidth',1);
plot(1:100,xpos(1,:)-x(1,:),'r','linewidth',1);
set(gca,'fontname','monospaced');
legend('先驗誤差','測量誤差','後驗誤差','location','best');
我對卡爾曼濾波的簡單直白的理解
先用感性的語言來描述一下我的理解 卡爾曼濾波是基於前乙個可信度以及輸入值與 值的偏差來對當前可信度進行計算,得到可信度後計算出當前值。卡爾曼模型認為,輸入值的誤差是呈正態分佈的。貼matlab 具體內容可以看注釋。clear close all hold on grid on axis equal ...
學習卡爾曼濾波的一些理解
1.卡爾曼濾波是貝葉斯濾波的一種具體實現形式,因此先把貝葉斯濾波弄明白,再學習卡爾曼濾波會感覺自然一些 個人感覺,貝葉斯濾波的核心就是貝葉斯公式,也即條件概率公式,其目的就是利用已有資訊去推測新的資訊 關於貝葉斯濾波的一些資料 a.b.2.卡爾曼濾波目的是融合目標的觀測資訊和 資訊,然後得出對目標狀...
卡爾曼濾波五個公式各個引數的意義
系統的狀態方程為 這個狀態方程是根據上一時刻的狀態和控制變數來推測此刻的狀態,wk 1是服從高斯分布的雜訊,是 過程的雜訊,它對應了 xk 中每個分量的雜訊,是期望為 0,協方差為 q 的高斯白雜訊wk 1 n 0,q q即下文的過程激勵雜訊q.觀測方程為 vk是觀測的雜訊,服從高斯分布,vk n ...