『壹』 請大家奉獻一個效果好的卡爾曼濾波的matlab程序，最好有說明，重謝！
我現在也在研究kalman，這是最新發現的一個程序，我做的標注，有問題一塊研究
function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)
% Kalman filter.
% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)
%
% INPUTS:
% y(:,t) - the observation at time t 在時間t的觀測
% A - the system matrix A 系統矩陣
% C - the observation matrix C 觀測矩陣
% Q - the system covariance Q 系統協方差
% R - the observation covariance R 觀測協方差
% init_x - the initial state (column) vector init_x 初始狀態（列）向量
% init_V - the initial state covariance init_V 初始狀態協方差
%
% OPTIONAL INPUTS (string/value pairs [default in brackets]) 選擇性輸入（字元串/值 對【默認在括弧中】）
% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] 在時間t，m意味著利用 m模型參數 [ones(1,T) ]
%
% In this case, all the above matrices take an additional final
% dimension, 在這種情況下,上述矩陣採用附加的維數
% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). 例如
% However, init_x and init_V are independent of model(1).
% init_x and init_V相對於模型1是獨立的
% 'u' - u(:,t) the control signal at time t [ [] ]
% 在時間t的控制信號
% 'B' - B(:,:,m) the input regression matrix for model m
% 對於模型m的，輸入回歸矩陣
% OUTPUTS (where X is the hidden state being estimated) 輸出（其中X是被估計的隱藏狀態）
% x(:,t) = E[X(:,t) | y(:,1:t)]
% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]
% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2
% loglik = sum{t=1}^T log P(y(:,t))
%
% If an input signal is specified, we also condition on it: 如果一個輸入信號是特定的,我們的條件
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]
% If a model sequence is specified, we also condition on it:
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]
[os T] = size(y);
ss = size(A,1); % size of state space
% set default params
model = ones(1,T);
u = [];
B = [];
ndx = [];
args = varargin;
nargs = length(args);
for i=1:2:nargs
switch args{i}
case 'model', model = args{i+1};
case 'u', u = args{i+1};
case 'B', B = args{i+1};
case 'ndx', ndx = args{i+1};
otherwise, error(['unrecognized argument ' args{i}])
end
end
x = zeros(ss, T);
V = zeros(ss, ss, T);
VV = zeros(ss, ss, T);
loglik = 0;
for t=1:T
m = model(t);
if t==1
%prevx = init_x(:,m);
%prevV = init_V(:,:,m);
prevx = init_x;
prevV = init_V;
initial = 1;
else
prevx = x(:,t-1);
prevV = V(:,:,t-1);
initial = 0;
end
if isempty(u)
[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);
else
if isempty(ndx)
[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...
'initial', initial, 'u', u(:,t), 'B', B(:,:,m));
else
i = ndx{t};
% over all elements; only some will get updated
x(:,t) = prevx;
prevP = inv(prevV);
prevPsmall = prevP(i,i);
prevVsmall = inv(prevPsmall);
[x(i,t), smallV, LL, VV(i,i,t)] = ...
kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...
'initial', initial, 'u', u(:,t), 'B', B(i,:,m));
smallP = inv(smallV);
prevP(i,i) = smallP;
V(:,:,t) = inv(prevP);
end
end
loglik = loglik + LL;
end
『貳』 下面卡爾曼濾波的matlab程序語句，請問每個語句是什麼意思呀
%這個問題我已經回答過了，下面是我以前的回復
clear
N=200;%取200個數
w(1)=0;
w=randn(1,N);%產生一個1×N的行向量，第一個數為0，w為過程雜訊（其和後邊的v在卡爾曼理論里均為高斯白雜訊）
x(1)=0;%狀態x初始值
a=1;%a為狀態轉移陣，此程序簡單起見取1
for k=2:N
x(k)=a*x(k-1)+w(k-1); %系統狀態方程，k時刻的狀態等於k-1時刻狀態乘以狀態轉移陣加雜訊（此處忽略了系統的控制量）
end
V=randn(1,N);%測量雜訊
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2; %此方程未用到Rxx
q3=std(w);
Rww=q3.^2; %Rvv、Rww分別為過程雜訊和測量雜訊的協方差(此方程只取一組數方差與協方差相同)
c=0.2;
Y=c*x+V;%量測方差，c為量測矩陣，同a簡化取為一個數
p(1)=0;%初始最優化估計協方差
s(1)=0;%s(1)表示為初始最優化估計
for t=2:N
p1(t)=a.^2*p(t-1)+Rww;%p1為一步估計的協方差，此式從t-1時刻最優化估計s的協方差得到t-1時刻到t時刻一步估計的協方差
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%b為卡爾曼增益，其意義表示為狀態誤差的協方差與量測誤差的協方差之比(個人見解)
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%Y(t)-a*c*s(t-1)稱之為新息，是觀測值與一步估計得到的觀測值之差，此式由上一時刻狀態的最優化估計s(t-1)得到當前時刻的最優化估計s(t)
p(t)=p1(t)-c*b(t)*p1(t);%此式由一步估計的協方差得到此時刻最優化估計的協方差
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');%作圖，紅色為卡爾曼濾波，綠色為量測，藍色為狀態
%整體來說，此卡爾曼程序就是一個循環迭代的過程，給出初始的狀態x和協方差p，得到下一時刻的x和p，循環帶入可得到一系列的最優的狀態估計值，此方法通常用於目標跟蹤和定位。
%本人研究方向與此有關，有興趣可以交流下
『叄』 matlab 卡爾曼濾波器 數據處理
卡爾曼濾波不大會
如果10*5的矩陣為A
可以用atan(A)/pi+0.5
不過這種歸一化的方法容易使得數據間差異很小
還可以這樣，
maxnum=max(max(A));
minnum=min(min(A));
B=(A-minnum)/(maxnum-minnum);%B為A歸一化到(0,1)後的矩陣
『肆』 卡爾曼濾波怎麼在matlab裡面運算
你可以直接調用matlab 里的kalman()函數進行卡爾曼濾波運算
方程格式如下
[kest,L,P] = kalman(sys,Qn,Rn,Nn)
sys 表示系統狀態方程
Qn,Rn分別是Q矩陣和R矩陣
Nn是觀測雜訊和系統雜訊的協方差
『伍』 哪位大神有GPS與捷聯慣導組合導航的卡爾曼濾波演算法的matlab模擬程序
在下面的模擬的代碼中，理想的觀測量不是真實數據，而是自生成的正弦波數據，在真實的應用場景中，應該是一系列的參考數據。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡爾曼濾波器在INS-GPS組合導航中應用模擬
% Author : lylogn
% Email : [email protected]
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 參考文獻:
% [1]. 鄧正隆. 慣導技術, 哈爾濱工業大學出版社.2006.
clear all;
%% 慣性-GPS組合導航模型參數初始化
we = 360/24/60/60*pi/180; %地球自轉角速度，弧度/s
psi = 10*pi/180; %psi角度 / 弧度
Tge = 0.12;
Tgn = 0.10;
Tgz = 0.10; %這三個參數的含義詳見參考文獻
sigma_ge=1;
sigma_gn=1;
sigma_gz=1;
%% 連續空間系統狀態方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;
-we*sin(psi) 0 0 0 1 0 0 1 0;
we*cos(psi) 0 0 0 0 1 0 0 1;
0 0 0 -1/Tge 0 0 0 0 0;
0 0 0 0 -1/Tgn 0 0 0 0;
0 0 0 0 0 -1/Tgz 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;]; %狀態轉移矩陣
B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%輸入控制矩陣
%% 轉化為離散時間系統狀態方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1;
[F,G]=c2d(A,B,T);
H=[1 0 0 0 0 0 0 0 0;
0 -sec(psi) 0 0 0 0 0 0 0;];%觀測矩陣
%% 卡爾曼濾波器參數初始化
t=0:T:50-T;
length=size(t,2);
y=zeros(2,length);
Q=0.5^2*eye(3); %系統雜訊協方差
R=0.25^2*eye(2); %測量雜訊協方差
y(1,:)=2*sin(pi*t*0.5);
y(2,:)=2*cos(pi*t*0.5);
Z=y+sqrt(R)*randn(2,length); %生成的含有雜訊的假定觀測值，2維
X=zeros(9,length); %狀態估計值，9維
X(:,1)=[0,0,0,0,0,0,0,0,0]'; %狀態估計初始值設定
P=eye(9); %狀態估計協方差
%% 卡爾曼濾波演算法迭代過程
for n=2:length
X(:,n)=F*X(:,n-1);
P=F*P*F'+ G*Q*G';
Kg=P*H'/(H*P*H'+R);
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
P=(eye(9,9)-Kg*H)*P;
end
%% 繪圖代碼
figure(1)
plot(y(1,:))
hold on;
plot(y(2,:))
hold off;
title('理想的觀測量');
figure(2)
plot(Z(1,:))
hold on;
plot(Z(2,:))
hold off;
title('帶有雜訊的觀測量');
figure(3)
plot(X(1,:))
hold on;
plot(X(2,:))
hold off;
title('濾波後的觀測量');
『陸』 MATLAB中實現擴展卡爾曼和無味卡爾曼濾波，想對一條簡單的曲線和直線進行濾波，
% INPUTS:
% y(:,t) - the observation at time t
% A - the system matrix
% C - the observation matrix
% Q - the system covariance
% R - the observation covariance
% init_x - the initial state (column) vector
% init_V - the initial state covariance
『柒』 基於Matlab，用Kalman濾波實現線性變化的電壓測量濾波處理。
%卡爾曼濾波
clear
N=800;
w(1)=0;
w=randn(1,N)
%系統預測的隨機白雜訊
x(1)=0;
a=1;
for
k=2:N;
x(k)=a*x(k-1)+w(k-1);
%系統的預測值
end
V=randn(1,N);
%測量值的隨機白雜訊
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;
%測量值
p(1)=0;
s(1)=0;
for
t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
%前一時刻X的相關系數
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
%卡爾曼增益
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
%經過濾波後的信號
p(t)=p1(t)-c*b(t)*p1(t);%t狀態下x(t|t)的相關系數
end
figure(1)
plot(x)
title('系統的預測值')
figure(2)
plot(Y)
title('測量值')
figure(3)
plot(s)
title('濾波後的信號')
『捌』 擴展卡爾曼濾波（EKF）演算法詳細推導及模擬（Matlab）
姓名：王柯禕
學號：20021110373T
轉自 ：https://blog.csdn.net/gangdanerya/article/details/105105611
【嵌牛導讀】介紹擴展卡爾曼濾波（EKF）演算法的詳細推導，局限性和MATLAB模擬。
【嵌牛鼻子】擴展卡爾曼濾波（EKF）
【嵌牛正文】
擴展卡爾曼濾波演算法 是解決非線性狀態估計問題最為直接的一種處理方法，盡管EKF不是最精確的」最優「濾波器，但在過去的幾十年成功地應用到許多非線性系統中。所以在學習非線性濾波問題時應該先從EKF開始。
EKF演算法是將非線性函數進行泰勒展開，然後省略高階項，保留展開項的一階項，以此來實現非線性函數線性化，最後通過卡爾曼濾波演算法近似計算系統的狀態估計值和方差估計值。
一、EKF演算法詳細推導
【注】EKF推導參考的是黃蔚的博士論文「CKF及魯棒濾波在飛行器姿態估計中的應用研究」，論文中EKF，UKF和CKF等演算法講解的都很詳細，值得一看。
我們把KF與EKF演算法拿出來對比可以發現：
二、EKF演算法局限性：
該演算法線性化會引入階段誤差從而導致濾波精度下降，同時當初始狀態誤差較大或系統模型非線性程度較高時，濾波精度會受到嚴重影響甚至發散。
需要計算雅克比矩陣，復雜，計算量大，影響系統的實時性，還會導致EKF演算法的數值穩定性差。
當系統存在模型失配，量測干擾，量測丟失，量測延遲或狀態突變等復雜情況時，EKF演算法魯棒性差。
三、Matlab模擬：
clear all;clc; close all;
tf = 50;
Q = 10;w=sqrt(Q)*randn(1,tf);
R = 1;v=sqrt(R)*randn(1,tf);
P =eye(1);
x=zeros(1,tf);
Xnew=zeros(1,tf);
x(1,1)=0.1;
Xnew(1,1)=x(1,1);
z=zeros(1,tf);
z(1)=x(1,1)^2/20+v(1);
zjian=zeros(1,tf);
zjian(1,1)=z(1);
for k = 2 : tf
%%%%%%%%%%%%%%%模擬系統%%%%%%%%%%%%%%%
x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1);
z(k) = x(:,k).^2 / 20 + v(k);
%%%%%%%%%%%%%%%EKF開始%%%%%%%%%%%%%%%
Xpre = 0.5*Xnew(:,k-1)+ 2.5*Xnew(:,k-1)/(1+Xnew(:,k-1).^2) + 8 * cos(1.2*(k-1));
zjian =Xpre.^2/20;
F = 0.5 + 2.5 * (1-Xnew.^2)/((1+Xnew.^2).^2);
H = Xpre/10;
PP=F*P*F'+Q;
Kk=PP*H'*inv(H*PP*H'+R);
Xnew(k)=Xpre+Kk*(z(k)-zjian);
P=PP-Kk*H*PP;
end
t = 2 : tf;
figure; plot(t,x(1,t),'b',t,Xnew(1,t),'r*'); legend('真實值','EKF估計值');
模擬結果：