‘壹’ 请大家奉献一个效果好的卡尔曼滤波的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估计值');
仿真结果：