交互式多模型算法卡尔曼滤波仿真
交互式多模型算法卡尔曼滤波仿真
1 模型建立
分别以加速度u=0、1、2代表三个不同的运动模型
状态方程x(k+1)=a*x(k)+b*w(k)+d*u
观察方程z(k)=c*x(k)+v(k)
其中，a=[1 dt;0 1],b=[dt^2/2;dt],d=[dt^2/2;dt],c=[1 0]
2 程序代码
由两个功能函数组成，imm_main用来实现imm 算法，move_model1用来生成仿真数据，初始化运动参数
function imm_main
%交互式多模型算法主程序
%初始化有关参数
move_model %调用运动模型初始化及仿真运动状态生成函数
load movedata %调入有关参数初始值（a b d c u position velocity pmeas dt tg q r x_hat p_var）
p_tran=[0.8 0.1 0.1;0.2 0.7 0.1;0.1 0.2 0.7];%转移概率
p_pri=[0.1;0.6;0.3];%模型先验概率
n=1:2:5; %提取各模型方差矩阵
k=0; %记录仿真步数
like=[0;0;0];%视然函数
x_hat_hat=zeros(2,3);%三模型运动状态重初始化矩阵
u_=zeros(3,3);%混合概率矩阵
c_=[0;0;0];%三模型概率更新系数
%数据保存有关参数初始化
phat=[];%保存位置估值
vhat=[];%保存速度估值
xhat=[0;0];%融合和运动状态
z=0;%量测偏差（一维位置）
pvar=zeros(2,2);%融合后估计方差
for t=0:dt:tg; %dt为为仿真步长；tg为仿真时间长度
k=k+1;%记录仿真步数
ct=0; %三模型概率更新系数
c_max=[0 0 0];%混合概率规范系数
p_var_hat=zeros(2,6);%方差估计重初始化矩阵,
%[x_hat_hat p_var_hat]=model_reinitialization(p_tran,p_pri,x_hat,p_var);%调用重初始化函数，进行混合估计，生成三个模型重初始化后的运动状态、方差
%混合概率计算
for i=1:3
u_(i,:)=p_tran(i,:)*p_pri(i);
end
for i=1:3
c_max=c_max+u_(i,:);
end
for i=1:3
u_(:,i)=u_(:,i)/c_max(i);
end
%各模型状态、方差重初始化
x_hat_hat=x_hat*u_;%运动状态重初始化
for j=1:3
for i=1:3
p_var_hat(:,n(j):n(j)+1)=
p_var_hat(:,n(j):n(j)+1)+(p_var(:,n(i):n(i)+1)+(x_hat(:,i)-x_hat_hat(:,j))*(x_hat(:,i)-x_hat_hat(:,j))')*u_(i,j);%方差混合估计
end
end
%各模型进行依次次kalman滤波
for i=1:3 %各模型进行依次次kalman滤波
% 模型条件滤波
x_hat(:,i)=a*x_hat_hat(:,i)+d*u(i);%一步状态预测
p_var(:,n(i):n(i)+1)=a*p_var_hat(:,n(i):n(i)+1)*a'+b*q*b';%一步状态预测方差
z=pmeas(k)-c*x_hat(:,i);%量测误差估计
s=c*p_var(:,n(i):n(i)+1)*c'+r;%量测方差
k_add=p_var(:,n(i):n(i)+1)*c'*inv(s);%kalman增益
x_hat(:,i)=x_hat(:,i)+k_add*z;
p_var(:,n(i):n(i)+1)=p_var(:,n(i):n(i)+1)-k_add*s*k_add';
%计算与当前模型匹配的视然函数
like(i)=1/sqrt(2*pi*s)*exp(-1/2*z^2/s);
end
% 模型概率更新
c_=p_tran*p_pri;
for i=1:3
c_(i)=c_(i)*like(i);
ct=ct+c_(i);
end
p_pri=c_/ct;%模型先验概率更新
xhat=x_hat*p_pri;%运动状态估计融合
for i=1:3 %方差融合
pvar=pvar+(p_var(:,n(i):n(i)+1)+(xhat-x_hat(:,i))*(xhat-x_hat(:,i))');
end
phat=[phat;xhat(1)];%位置估计值保存
vhat=[vhat;xhat(2)];%速度估计值保存
end
% 图形输出
t=0:dt:tg;
subplot(3,2,1);
%ylabel(‘Position (m)’);
plot(t,position);
grid;
title('真位置');
subplot(3,2,2);
plot(t,velocity);
grid;
title('真速度');
subplot(3,2,3);
plot(t,pmeas);
grid;
title('位置量测值');
subplot(3,2,4);
plot(t,vhat);
grid;
title('速度估计值');
subplot(3,2,5);
plot(t,phat);
grid;
title('位置估计值');
subplot(3,2,6);
plot(t,position-phat,t,velocity-vhat);
grid;
title('位置（蓝）、速度估计误差（绿）');
function move_model1
% 参数初始化
dt=0.1;%仿真步长（秒）
tg=200;%仿真持续时间（秒）
a=[1 dt;0 1];%状态转移矩阵
b=[dt^2/2;dt];%激励输入矩阵
d=[dt^2/2;dt];%机动加速度系数矩阵
c=[1 0];%量测矩阵
x=[0;0];%初始化状态矢量
x_hat=[x x x];%初始化状态估计矩阵（三个模型状态矢量综合考虑）
q=0.04;%过程随机噪声方差
r=36;%量测随机噪声方差
p_var=[b*q*b' b*q*b' b*q*b'];%初始化方差矩阵（三个模型状态矢量综合考虑）% 数据保存数组初始化
position=[];%真实位置
pmeas=[];%位置量测值
velocity=[];%真实速度
u=0:1:2;
%k=0:dt:tg
% 生成仿真模拟数据
for i=0:dt:tg
w=0.2*randn;%过程随机噪声，均值为0，方差为0.04
v=6*randn;%量测随机噪声，均值为0，方差为36
if i<=30
x=a*x+b*w+d*u(2);
elseif i>30&i<100
x=a*x+b*w+d*u(1);
elseif i>100&i<150
x=a*x+b*w+d*u(3);
elseif i>150&i<180
x=a*x+b*w+d*u(1);
else
x=a*x+b*w+d*u(3);
end
y=c*x+v;%量测值计算
position=[position;x(1)];%真实位置
pmeas=[pmeas;y];%位置量测值
velocity=[velocity;x(2)];%真实速度
end
save movedata a b d c u position velocity pmeas dt tg q r x_hat p_var
3 运行结果
图一：
机动方式： 0-30秒：u=1； 30-100秒：u=0； 100-150秒：u=2； 150-180秒：u0； 180-200秒：u=2 过程噪声方差：0.04 量测噪声方差：36
050100150200
05000
10000
15000真位置
050100150200
0100
200
真速度
50
100
150
200
-1012
4
位置量测值0
50
100
150
200
100
200速度估计值
50
100
150
200
05000
1000015000位置估计值
050100150200
-10
10
位置（蓝）、速度估计误差（绿）
图一
图二：
机动方式： 0-30秒：u=2； 30-100秒：u=0； 100-150秒：u=1； 150-180秒：u=0； 180-200秒：u=2 过程噪声方差：4 量测噪声方差：100
050100150200
05000
10000
15000真位置
050100150200
0100
200真速度
50
100
150
200
-1012
4
位置量测值0
50
100
150
200
100
200速度估计值
50
100
150
200
05000
1000015000位置估计值
050100150200
-10
10
位置（蓝）、速度估计误差（绿）
图二
卡尔曼滤波算法总结
Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q
几种卡尔曼滤波算法理论
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点：（1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵或量测矩阵H也不能确切建立。如果所建立 的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。 在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。 输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列 {C k}，再由{C讣推算出最佳增益矩阵K,使得增益矩阵K不断地与实际量测数据 {C k} 相适应。
卡尔曼滤波算法(C--C++两种实现代码)
卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_ #if _MSC_VER > 1000 #pragma once #endif// _MSC_VER > 1000 #include
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_) ============================kalman.cpp=============== ================= #include "kalman.h" #include
卡尔曼滤波算法总结
卡尔曼滤波算法总结-标准化文件发布号：（9556-EUATWK-MWUB-WUNN-INNUL-DDQTY-KII
2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }
首先是卡尔曼滤波的5个方程： (|1)(1|1)() X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)')Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1))X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_0 1_0 Angle dt Angle dt Q bias Q bia o s Gyr -= + 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)() X k k AX k k Bu k -=--+ (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
卡尔曼滤波中文
处理线性滤波以及预测问题的一种新途径R.E.Kalman1引言通讯与控制中的理论与实际问题中有很重要的一类具有统计性质。 这样的问题有：（1）、随机信号的预测；（2）、从随机噪声中分离随机信 号；（3）、在有噪声的情况下探测已知形式的信号（脉冲、正弦波）。 在Wiener开拓性的工作中，他证明[1]从问题（1）和问题（2）可导 出所谓Wiener-Hopf积分方程；他同样给出了解决具有实际重要意义的特 殊情况——定态统计和有理数频谱——之积分方程的方法（频谱因式分解）。 在Wiener的基础性工作之后出现了许多延伸和推广。Zadeh与 Ragazzini给出了有限存储器情况的解[2]。Bode和Shannon[3]同时独 立的给出上述情况的解，并且给出了简化的求解方法。Booton讨论了非定 态统计Wiener-Hopf方程[4]。这些结果现在都写入了标准教科书中[5,6]。 最近Darlington[7]沿着这些主线给出了一种稍微有些不同的方法。对抽 样信号的延伸，参见Franklin[8]和Lees[9]的工作。基于Wiener-Hopf方 程（同样应用于非定态问题，尽管前述方法一般并非如此）特征函数的 方法由Davis[10]开创并被许多其他人应用，例如Shinbrot[11],Blum[12], Pugachev[13],Solodovnikov[14]. 在所有这些工作中，目标都是获取一个线性动力系统的明确说明 （Wiener滤波器），由此可以完成预测、分离或者探测随机信号。 现有求解Wiener问题的方法受制于若干限制，这样就使得它们的实际 用处收到削弱： 1.最佳的滤波器由其脉冲响应具体指定。由这些数据合成滤波器并非易 事。 2.数值确定最佳的脉冲响应往往十分复杂并且不很适合机器计算。这种 情况随着问题复杂度的增加而迅速变得更为糟糕。 11引言2 3.重要的推广（如增长存储器滤波器、非定态预测）需要新的推导过 程，经常给非专业人士带来相当大的困难。 4.这些推导过程的数学部分并不透明。基本假设及其后果趋于模糊。 本文回避上述困难，提出看待这些问题的整个集合的新方式。以下是 本文的亮点： 5.最佳估计和正交投影。Wiener问题是以条件分布与期望的观点处理 的。这样，Wiener理论的基本事实可以迅速获取；结果的范围以及基 本假设可以清楚的显现出来。可以看到所有的统计计算以及结果都基 于一阶和二阶平均；不需要其他的统计数据。这样一来困难（4）便被 排除。这种方法在概率论中为人们所熟知（见Doob[15]第148至155 页以及Lo`eve[16]第455至464页），但在工程上还没有大量的应用。 6.随机过程模型。继前人之后，尤其是Bode与Shannon[3],任意随机 信号可以被表示（直到二阶平均统计性质）为线性动力系统受独立或 不相关随机信号（“白噪声”）激励后的输出。这是工程上应用Wiener 理论的标准手法[2,3,4,5,6,7]。这里用到的方法与传统方法相比只 在线性动力系统的描述方法上不同。我们将强调状态以及状态过渡； 换言之，线性系统将以一阶差分（或微分）方程组来刻画。为了利用 （5）中提到的简化，这种观点是自然的，也是必要的。 7.求解Wiener问题。使用状态——过渡方法，单独一次推导即覆盖多
卡尔曼滤波简介及其算法实现代码
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.sodocs.net/doc/e516164101.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
卡尔曼滤波基础知识
卡尔曼滤波 马尔可夫过程： 在随机理论中，把在某时刻的事件受在这之前事件的影响，其影响范围有限的随机过程，称为马尔可夫过程。一个事件受在它之前的事件的影响的深远程度，通常用在它之前的事件作为条件的概率来表达。受前一个事件的影响，简称为马尔可夫过程；受前两个事件的影响，称为二阶马尔可夫过程；受前三个事件的影响，称为三阶马尔可夫过程！ 卡尔曼滤波简介＋算法实现代码（转）： 最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。 现设线性时变系统的离散状态防城和观测方程为： X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1) Y(k) = H(k)·X(k)+N(k) 其中 X(k)和Y(k)分别是k时刻的状态矢量和观测矢量 F(k,k-1)为状态转移矩阵 U(k)为k时刻动态噪声
T(k,k-1)为系统控制矩阵 H(k)为k时刻观测矩阵 N(k)为k时刻观测噪声 则卡尔曼滤波的算法流程为： 1 2预估计X(k)^= F(k,k-1)·X(k-1) 3计算预估计协方差矩阵C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)' Q(k) = U(k)×U(k)' 4计算卡尔曼增益矩阵 K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1) R(k) = N(k)×N(k)' 5更新估计 X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 6计算更新后估计协防差矩阵 C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' 7X(k+1) = X(k)~ C(k+1) = C(k)~ 重复以上步骤 其c语言实现代码如下： #include "stdlib.h" #include "rinv.c" int lman(n,m,k,f,q,r,h,y,x,p,g) int n,m,k; double f[],q[],r[],h[],y[],x[],p[],g[]; { int i,j,kk,ii,l,jj,js; double *e,*a,*b; e=malloc(m*m*sizeof(double)); l=m;
一种卡尔曼滤波自适应算法概要
- 23 - 一种卡尔曼滤波自适应算法 黄波郑新星刘凤伟 (中船重工750试验场,云南昆明 650051
【摘要】自适应滤波是指随着外部信号的变化,滤波器能够自我调节滤波参数,使得滤波器的某一性能指标达到最优。文章以卡尔曼滤波理论为基础,给出一种新的自适应卡尔曼滤波算法。 【关键词】数字信号处理;卡尔曼滤波器;MATLAB 【中图分类号】TP391【文献标识码】A【文章编号】1008-1151(201203-0023-02 An adaptive Algorithm on Kalman Filtering Abstruct:Adaptive-filtering means the filter could adjust filtration parameters by itself and make some performance index optimal when the external signals vary. This paper will give a new Kalman filter algorithm whose base is Kalman filter theory. Key word: Digital Signal Processing;Kalman Filter;MATLAB 1 引言 自适应滤波理论是20世纪60年代开始发展起来的。它 是现代信号处理技术的重要组成部分,对复杂信号的处理具 有独特的功能。自适应卡尔曼滤波算法在很多理论和工程实 践中都取得了广泛的应用[1][2][3]。卡尔曼滤波理论的建立的 标志是1960年卡尔曼发表的用递归的方法解决离散数据线 性滤波问题的论文。在那之后,得益于数字计算技术的进步, 卡尔曼滤波器就成为了推广研究和应用的主题,并且在自主 或协助导航领域取得了长足的发展[4][5]。常见的自适应滤波器
Kalman滤波算法
Kalman 滤波算法 姓名：刘金强 专业：控制理论与控制工程 学号：2007255 ◆实验目的： （1）、掌握klman 滤波实现的原理和方法 （2）、掌握状态向量预测公式的实现过程 （3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求： 问题： F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n). ◆实验原理： 初始条件： 1?(1)x =E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)} 输入观测向量过程： 观测向量序列={y(1),…………y(n)} 已知参数： 状态转移矩阵F(n+1,n) 观测矩阵C(n) 过程噪声向量的相关矩阵1()Q n 观测噪声向量的相关矩阵2()Q n 计算：n=1,2,3,………………. G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+ Kalman 滤波器是一种线性的离散时间有限维系统。Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。这意味着，kalman 滤波器是状态向量x(n)的线性最小方差估计。 ◆实验结果： ◆程序代码： （1）主程序
卡尔曼滤波的原理说明(通俗易懂)
为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。 另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。 首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。 然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance 来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg =0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23) =24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5 =2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！ 下面就要言归正传，讨论真正工程系统上的卡尔曼。
卡尔曼(kalman)滤波算法特点及其应用
Kalman滤波算法的特点： （1）由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入/输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳随机过程的滤波，而且特别适用于非平稳或平稳马尔可夫序列或高斯-马尔可夫序列的滤波，所以其应用范围是十分广泛的。 （2）Kalman滤波算法是一种时间域滤波方法，采用状态空间描述系统。系统的过程噪声和量测噪声并不是需要滤除的对象，它们的统计特征正是估计过程中需要利用的信息，而被估计量和观测量在不同时刻的一、二阶矩却是不必要知道的。 （3）由于Kalman滤波的基本方程是时间域内的递推形式，其计算过程是一个不断地“预测-修正”的过程，在求解时不要求存储大量数据，并且一旦观测到了新的数据，随即可以算的新的滤波值，因此这种滤波方法非常适合于实时处理、计算机实现。 （4）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出，从而可以减少实时在线计算量。在求滤波器增益矩阵时，要求一个矩阵的逆，它的阶数只取决于观测方程的维数，而该维数通常很小，这样，求逆运算是比较方便的。另外，在求解滤波器增益的过程中，随时可以算出滤波器的精度指标P，其对角线上的元素就是滤波误差向量各分量的方差。 Kalman滤波的应用领域 一般地，只要跟时间序列和高斯白噪声有关或者能建立类似的模型的系统，都可以利用Kalman滤波来处理噪声问题，都可以用其来预测、滤波。Kalman滤波主要应用领域有以下几个方面。 （1）导航制导、目标定位和跟踪领域。 （2）通信与信号处理、数字图像处理、语音信号处理。 （3）天气预报、地震预报。 （4）地质勘探、矿物开采。 （5）故障诊断、检测。 （6）证券股票市场预测。 具体事例： （1）Kalman滤波在温度测量中的应用； （2）Kalman滤波在自由落体运动目标跟踪中的应用； （3）Kalman滤波在船舶GPS导航定位系统中的应用； （4）Kalman滤波在石油地震勘探中的应用； （5）Kalman滤波在视频图像目标跟踪中的应用；
卡尔曼滤波算法及MATLAB实现
基于matlab的卡尔曼信号滤波设计 卡尔曼滤波的基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 语音信号在较长时间内是非平稳的，但在较短的时间内的一阶统计量和二阶统计量近似为常量，因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。假定语音信号可看成由一AR模型产生： 时间更新方程： 测量更新方程： K(t)为卡尔曼增益，其计算公式为： 其中 、分别为过程模型噪声协方差和测量模型噪声协方差，测量协方差可以通过观测得到， 则较难确定，在本实验中则通过与两者比较得到。 由于语音信号短时平稳，因此在进行卡尔曼滤波之前对信号进行分帧加窗操作，在滤波之后对处理得到的信号进行合帧，这里选取帧长为256，而帧重叠个数为128； 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号：
原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后的信号 MATLAB程序实现如下： %%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear; clc; %%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% loadvoice.mat y=m1(2,:); x=y+0.08*randn(1,length(y)); %%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1); subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号'); subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号');
卡尔曼滤波算法总结
2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }
首先是卡尔曼滤波的5个方程： (|1)(1|1)()X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)') Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1)) X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_01_0 Angle dt Angle dt Q bias Q bia o s Gyr -=+ 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)()X k k AX k k Bu k -=--+ ()|1X k k -为2维列向量 _Angle Q bias ，A 为2维方阵101dt -，()|-11X k k -为2维列向量_Angle Q bias ，B 为2维列向量0dt ， ()u k 为Gyro (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
卡尔曼滤波算法(KF)
卡尔曼滤波器Kalman Filter (zz) 关键词：卡尔曼滤波器Kalman Filter 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Predict ion Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.sodocs.net/doc/e516164101.html,/~welch/kalman/media/pdf/Kalman1960.pdf 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后
卡尔曼滤波算法
卡尔曼滤波器的介绍 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下 一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位） 。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻
的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25 度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现 在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的
卡尔曼滤波算法(C--C++两种实现代码)
卡尔曼滤波算法实现代码
C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_
#if _MSC_VER > 1000 #pragma once #endif // _MSC_VER > 1000
#include
class kalman { public: void init_kalman(int x,int xv,int y,int yv); CvKalman* cvkalman; CvMat* state; CvMat* process_noise; CvMat* measurement; const CvMat* prediction; CvPoint2D32f get_predict(float x, float y);
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman();
};
#endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_)
============================kalman.cpp=============== =================
#include "kalman.h" #include
/* tester de printer toutes les valeurs des vecteurs */ /* tester de changer les matrices du noises */ /* replace state by cvkalman->state_post ??? */
CvRandState rng; const double T = 0.1; kalman::kalman(int x,int xv,int y,int yv) {
cvkalman = cvCreateKalman( 4, 4, 0 ); state = cvCreateMat( 4, 1, CV_32FC1 ); process_noise = cvCreateMat( 4, 1, CV_32FC1 ); measurement = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1;
卡尔曼滤波算法C语言实现
卡尔曼滤波算法及C 语言实现 摘要：本文着重讨论了卡尔曼滤波器的原理，典型算法以及应用领域。清晰地阐述了 kalman filter 在信息估计方面的最优性能。着重介绍简单kalman filter algorithm 的编程，使 用kalman filter 的经典5个体现最优化递归公式来编程。通过c 语言编写程序实现kalman filter 的最优估计能力。 关键词：kalman filter ；最优估计；C 语言 1 引言 Kalman Filter 是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估计动态系统的状态。起源于Rudolf Emil Kalman 在1960年的博士论文和发表的论文《A New Approach to Linear Eiltering and Prediction Problems 》（《线性滤波与预测问题的新方法》）。并且最先在阿波罗登月计划轨迹预测上应用成功，此后kalman filter 取得重大发展和完善。它的广泛应用已经超过30年，包括机器人导航，控制。传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等，近年来更被广泛应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2 kalman filter 最优化递归估计 Kalman filter 是一个“optimal recursive data processing algorithm （最优化递归数据处理 方法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的方法。而kalman filter 最为核心的内容是体现它最优化估计和递归特点的5条公式。举一个例子来详细说明 5条公式的物理意义。 假设我们要研究的对象是某一个房间的温度信号。对于室温来说，一分钟内或一小段时间内的值是基本上不变的或者变化范围很小。也就是说时刻的温度和时刻的温度 1t 1T 2t 基本不变，即。在这个过程中，因为毕竟温度还是有所改变的，设有几度的偏差。 2T 12T T =我们把这几度的偏差看成是高斯白噪声，也就是说，。除 )(t w 0)]([=t w E 2 )]([σ=t w D 此之外我们在用一个温度计来实时测量房间的温度值，但由于量具本身的误差，所测得 Z 的温度值也是不准确的，也会和实际值偏差几度，把这几度的偏差看成是测量噪声。 )(t v 即满足，。 0)]([=t v E 2 1)]([σ=t v D 此时我们对于这个房间的温度就得到了两个数据。一个是你根据经验得到的经验值 ，一个是从温度计上得到的测量值，以及各自引入的高斯白噪声。下面就具体讲 12T T =Z
相关文档
- 几种卡尔曼滤波算法理论
- 卡尔曼滤波算法(含详细推导)
- 一种新型自适应卡尔曼滤波算法_高勤
- 基本滤波算法-维纳滤波+卡尔曼滤波+自适应滤波
- 卡尔曼滤波算法(含详细推导)
- 卡尔曼滤波器的自适应算法
- 卡尔曼滤波算法(含详细推导)
- 基本滤波算法-维纳滤波+卡尔曼滤波+自适应滤波
- 基于自适应Kalman滤波算法的联邦滤波器(1)
- 几种卡尔曼滤波算法理论
- 卡尔曼滤波算法(含详细推导)
- 基于自适应卡尔曼滤波算法确定汽车参考车速
- 的卡尔曼滤波算法
- 一种卡尔曼滤波自适应算法概要
- 自适应卡尔曼滤波算法改进与仿真
- 新型自适应Kalman滤波算法及其应用
- 渐消卡尔曼滤波器的最佳自适应算法及其应用
- 经典的卡尔曼滤波算法