卡尔曼滤波算法 卡尔曼滤波算法c语言

标量卡尔曼滤波器

y=zeros(2,length);

早在20世纪40年代，有人开始用状态变量模型来研究随机过程，到60年代初，由于空间技术的发展，为了解决对非平稳、多输入输出随机序列的估计问题，卡尔曼（R.E.Kalman）提出了递推估计理论。卡尔曼于1960年首先解决了对离散时间的线性滤波递推解，通常称为离散卡尔曼滤波器，或简称为卡尔曼滤波器。具有连续时间的线性滤波问题由卡尔曼等人于1961年解决，通常称为连续时间卡尔曼滤波，或卡尔曼-布西滤波。在理论上具有重要价值。它是在滤波的基础上发展出来的一种新方法。虽然卡尔曼滤波与滤波都属于最小均方误滤波，但它们所要求已知条件、计算方法及适用范围等并不一样。卡尔曼滤波不同于其它滤波方法，它是采用“状态”描述物理系统，利用“状态空间”来反映系统变化的内在规律，并用观测量来进行修正（或滤波）。这在线性滤波领域中是一项重大的突破。目前在许多领域如自动控制、导航、卫星轨道测定等都得到成功的应用，它的适应性较广，不管对时变、时不变，平稳或非平稳情况都能适用，而且还能推广到非线性模型。卡尔曼滤波还有一个重要的特点是：采用递推式的滤波方法，这种方法不要求保存过去的历史数据，只要新数据测得后，结合前一时刻已求出的估计值及系统本身的状态方程，按一定的递推方式即可求得新的估计值。其结果和用过去全部数据从头作滤波计算效果一样。这就特别有利于在计算机上进行处理。

卡尔曼滤波算法 卡尔曼滤波算法c语言

卡尔曼滤波算法 卡尔曼滤波算法c语言

卡尔曼滤波算法 卡尔曼滤波算法c语言

前面我们在时域上讨论了滤波，要求设计一个滤波器h（n），使其滤波输出与期望输出之间误平方和最小，它只适用于平稳随机序列，而且需要用到过去的和当前的所有数据，因此有一定的局限性。

下面我们先用一具体例子来比较维纳和卡尔曼算法的基本思路。在同样条件下，对信号s作n-1次观测，得到n-1个观测值x（1），x（2），…，x（n-1），其中x（k）=s+v（k）（k=1，2，…，n-1），v（k）是观测误，服从相同正态分布N（0， ）。现在用n-1个观测值的均值

来估计s时，精度要高，这是因为 服从均值为0，方为 的正态分布。显然n越大，精度越高。用n-1次观测的均值来估计s实质上也是一种滤波运算，它尽量使估计误减少。下面如又增加一次观测，得到第n个观测值x（n）。那么，现在怎样估计s呢？若根据上面的计算法，有

来估计s就行了。然而，这需要保存原来n-1个数据x（1），x（2），…，x（n（1），n越大，保存的数据就越多，求和时加法次数也越多，这就相当于维纳等人的思路。而卡尔曼等人的方法是：设n-1次观测后的估计值 已算出，那么

所以在得到第n个观测值x（n）后的均值应为

这样，只要保存前一次估计值 ，测得x（n）后，由上式马上能算出新的估计值 。上式中 是新数据与原估计值的值，称作新息（Innovation），1/n称为增益（加权）因子。新息经过增益“放大”或“缩小”后补充到原估计值上，就得新估计值。这显然是递推运算，它特别适合在计算机上实现，即具有以下几个特点：

（2）用递推法计算，不需要知道全部过去的值，用状态方程描述状态变量的动态变化规律，因此信号可以是平稳的，也可以是非平稳的，即卡尔曼滤波适用于非平稳过程。

（3）卡尔曼滤波采取的误准则仍为估计误的均方值最小。

这一节我们首先利用上述2.8节的结果来阐述卡尔曼滤波的递推算法。

为使卡尔曼滤波过程的物理意义明确，本节采用下列符号：

（1）用 代替 ，表示用n时刻及以前所有数据即｛x（i）；-∞＜i≤n｝对s（n）所作的线性估计；

（2）用 代替 ，表示用（n-1）时刻及其以前所有数据｛x（i）；-∞＜i≤（n-1）｝对s（n-1）所作的线性估计。

2.8节推导的因果IIR解最适合于用递归滤波器来实现。由式（2-73）写出滤波器的分方程

将式（2-75）代入上式得到

这就是因果IIR滤波器的递推计算公式——卡尔曼滤波器标准形式。卡尔曼滤波器实际上只不过是滤波的一种递推计算方法。

式（2-77）具有很明确的物理意义：

（1）设在n时刻数据x（n）到来之前已经得到了估计值 ，那么就有条件根据模型方程式（2-57）对s（n）进行预测，预测值为

白噪声w（n）不能对s（n）作预测；

（2）根据测量方程（2-58）由 对测量值x（n）进行预测，hold off;预测值为

白噪声未参加预测；

（3）x（n）到来后，将预测值 与x（n）进行比较，得到预测误

α（n）代表x（n）中所含的无法预测的信息，称为新息；

（4）选择适当的系数Gn对新息进行加权，作为对预测值 的修正值。修正后得到对信号的估计

在这里，不同时间的加权系数是不同的，故用带下标的符号Gn表示。相应的均方误最小，即

根据式（2-82）所示的最小均方误准则来求取修正加权系数Gn。令ξ（n）对Gn的偏导数等于零，得

由此得到

令地球物理信息处理基础

表示信号的一步预测误，又令

表示相应的预测误功率。注意到

及地球物理信息处理基础

考虑到v（n）与e1（n）不相关，故式（2-83）变为

由此得到

将上式写成另一种形式

由该式看出，预测误功率越大，加权系数就越大。这是很自然的，因为预测越不准确，利用新息进行的修正就应该越多。

在式（2-83）中，考虑到 ，故有E［e（n）x（n）］=0，即E［e（n）s（n）］=-E［e（n）v（n）］/c。

另一方面

故有

将式（2-86）代入上式并注意到v（n）与e1（n）不相关，得到

ξ（n）=GnR/c

将式（2-88）中的GnR=cP（n）［1-cGn］代入上式，得到

该式说明，由于利用新息对信号预测值进行了修正，故最小均方误比预测误Pdot[0] = Q_angle - P[0][1] - P[1][0];功率低一个数值cGnP（n）。

由式（2-85），有

综上推导，将几个重要公式汇集如下：

卡尔曼滤波过程实际上是获取解的递推运算过程，这一过程从某个初始状态启动，经过迭代运算，最终达到稳定状态，即滤波状态。设已经有了初始值 和ξ（0），这样便可由式（2-）中第二、三、四式计算P（1）、G1、ξ（1），然后再由式计算 。已知 和ξ（1），便作为下一轮迭代运算的已知数据。在递推运算过程中，随着迭代次数n的增加，ξ（n）将逐渐下降，直到最终趋近于某个稳定值ξ0。这时

ξ（n）=ξ（n-1）=ξ0

为求得这个稳定值，将式（2-）中第二、三式代入第四式，得到

解此方程即可求出ξ0。

由式（2-）中第二、三式看出，ξ（0）、P（1）、G1之间有密切关系，所以其中任一个都可选作为初始值。现选G1作为初始值。另一初始值是 。由于

故 的合理选择应使ξ（0）最小化。为此，令

由此得

下面讨论G1的选择。G1的选择应使ξ（1）最小，由式（2-93）得到E［e（n）x（n）］=0，故有E［e（1）x（1）］=0，即

E［s（1）x（1）］-G1E［x2（1）］=0

考虑到 和E［x2（1）］=E｛［cs（1）+v（1）］2｝ ，这里， 和 分别是信号s（n）和噪声v（n）的方，由上式得到

其中

故有

卡尔曼滤波的应用

/

卡尔曼滤波一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以估计也可看作是滤波过程。

#define KalmanFilter_h

应用：

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以估计也可看作是滤波过程。

在雷达中人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计，也可以是对于将来位置的预测，也可以是对过去位置的插值或平滑。

谁能给我讲解一下卡尔曼滤波，我最近在用mpu6050，把陀螺仪和加速度的值通过卡尔曼滤波融合。求C程序!

title('带有噪声的观测量');

给你arduino的卡尔曼滤波融合算法，非原创，我只是封装了算法。

卡尔曼滤波，顾名思义，滤波就是滤波感兴趣信息中的噪声，并给出感兴趣信息的一种估计；

H文件：

KalmanFilter.h

Non-original

Author: 我们可以用这些公式对任何线性系统建立的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。x2d

Copyright (c) 2012 China

/

#ifndef KalmanFilter_h

class KalmanFilter

{public:

KalmanFilter();

angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度（弧度值）

gyro_m:陀螺仪测量的角速度值（弧度值）

dt:采样时间(s)

outAngle:卡尔曼融合计算出的角度（弧度值）

outAngleDot:卡尔曼融合计算出的角速度（弧度值）

/

void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

private:

double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

double angle, angle_dot;

double P[2][2];

double Pdot[4];

CPP文件：

KalmanFilter.cpp

Non-original

Author: x2d

Copyright (c) 2012 China

/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()

{C_0 = 1.0f;

Q_angle = 0.001f;

Q_gyro = 0.003f;

R_angle = 0.5f;

q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;

angle = angle_dot = 0.0f;

P[0][0] = 1.0f;

P[0][1] = 0.0f;

Pdot[0] = 0.0f;

Pdot[1] = 0.0f;

Pdot[2] = 0.0f;

Pdot[3] = 0.0f;

}void KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)

{/

Serial.print("angle_m = ");

Serial.print(angle_m);

Serial.print(";");

Serial.print("gyro_m = ");

Serial.print(gyro_m);

Serial.print(";");

/

angle+=(gyro_m-q_bias) dt;

angle_err = angle_m - angle;

Pdot[1] = -P[1][1];

Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] dt;

P[1][0] += Pdot[2] dt;

P[1][1] += Pdot[3] dt;

PCt_0 = C_0 P[0][0];

PCt_1 = C_0 P[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 P[0][1];

P[0][0] -= K_0 t_0;

P[0][1] -= K_0 t_1;

P[1][0] -= K_1 t_0;

P[1][1] -= K_1 t_1;

angle += K_0 angle_err;

q_bias += K_1 angle_err;

angle_dot = gyro_m-q_bias;

outAngle = angle;

outAngleDot = angle_dot;

Serial.print("angle = ");

Serial.print(angle);

Serial.print(";");

Serial.print("angle_dot = ");

Serial.print(angle_dot);

Serial.print(";");

/

}#endif

卡尔曼滤波的详细原理

P[1][1] = 1.0f;

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用.

表达式

X(k)=A X(k-1)+B U(k)+W(k)

背景

斯坦利·施密特(Stanley Schmidt)首次实

现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

传统的滤波方法，只能是在有用信号与噪声具有不同频带的条件下才能实现．20世纪40年代，N．维纳和A．H．柯尔莫哥罗夫把信号和噪声的统计性质引进了滤波理论，在设信号和噪声都是平稳过程的条件下，利用化方法对信号真值进行估计，达到滤波目的，从而在概念上与传统的滤波方法联系起来，被称为维纳滤波。这种方法要求信号和噪声都必须是以平稳过程为条件。60年代初，卡尔曼(R．E．Kalman)和布塞(R． S．Bucy)发表了一篇重要的论文《线性滤波和预测 理论的新成果》，提出了一种新的线性滤波和预测理由论，被称之为卡尔曼滤波。特点是在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态或真实信号。

这种理论是在时间域上来表述的，基本的概念是：在线性系统的状态空间表示基础上，从输出和输入观测数据求系统状态的估计。这里所说的系统状态，是总结系统所有过去的输入和扰动对系统的作用的最小参数的，知道了系统的状态就能够与未来的输入与系统的扰动一起确定系统的整个行为。

卡尔曼滤波不要求信号和噪声都是平稳过程的设条件。对于每个时刻的系统扰动和观测误（即噪声），只要对它们的统计性质作某些适当的定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。例如在图像处理方面，应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。在对噪声作了某些统计性质的定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方最小的真实图像，使模糊的图像得到复原。

性质

①卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。

③当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方估计，也就是最小方估计。

形式

实例

卡尔曼滤波的一个典型实例是从一组有限的，对物置的，包含噪声的观察序列中预测出物体的坐标位置及速度。在很多工程应用(雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题。

应用地球物理信息处理基础

比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

是由kalman filter考虑时间非线性的动态系统，常应用于目标跟踪系统。

状态估计

状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功传感器早上用协方 表示，该分布的均值 是我们读取到的传感器数据。能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方估计、最小方估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、似然估计、随机逼近等方法也都有应用。

状态量

受噪声干扰的状态量是个随机量，不可能测得值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是估计。真实值与估计值之称为估计误。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理和非平稳的随机过程。

理论

卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。

哪位大神有GPS与捷联惯导组合导航的卡尔曼滤波算法的matlab仿真程序？

到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为

在下面的仿真的代码中，理想的观测量不是真实数据，而是自生成的正弦波数据，在真实的应用场景中，应该是一系列的参考数据。

% 卡尔曼滤波器在INS-GPS组合导航中应用仿真

% Author : lylogn

% Email : lylogn@gmail

% Company: BUAA-Dep3

% Time : 2013.01.06

% 参考文献:

% [1]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006.

clear all;

%% 惯性-GPS组合导航模型参数初始化

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 wesin(psi) -wecos(psi) 1 0 0 1 0 0;

-wesin(psi) 0 0 0 1 0 0 1 0;

wecos(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_gesqrt(2/Tge) 0 0 0 0 0;

0 0 0 0 sigma_gnsqrt(2/Tgn) 0 0 0 0;

0 0 0 0 0 sigma_gzsqrt(2/Tgz) 0 0 0;]';%输入控制矩阵

%% 转化为离散时间系统状态方程

[F,G]=c卡尔曼滤波已经有很多不同的实现，卡尔曼最初提出的形式一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。2d(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);

Q=0.5^2eye(3); %系统噪声协方

R=0.25^2eye(2); %测量噪声协方

y(1,:)=2sin(pit0.5);

y(2,:)=2cos(pit0.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)=FX(:,n-1);

P=FPF'+ GQG';

Kg=PH'/(HPH'+R);

X(:,n)=X(:,n)+Kg(Z(:,n)-HX(:,n));

P=(eye(9,9)-KgH)P;

end

%% 绘图代码

figure(1)

plot(y(1,:))

hold on;

plot(y(2,:))

title('理想的观测量');

figure(2)

plot(Z(1,:))

hold on;

plot(Z(2,:))

figure(3)

plot(X(1,:))

hold on;

plot(X(2,:))

title('滤波后的观测量');

无迹卡尔曼滤波结果为什么会不收敛与真实值

% X(k+1) = FX(k) + GW(k)

下面这段文字对卡尔曼的解释很形象，看看吧。

为了可以更加容易的理解卡尔曼滤波器，这里应用形象的描述方法讲解，不像参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度。设你对你的经验不是的相信，可能会有上下偏几度。我们把这些偏看成是高斯白噪声，也就是这些偏跟前后时间是没有关系的而且符合高斯分配。另外，我们在房间里放一个温度计，但是这个温度计也是不准确的，测量值会比实际值有偏。我们也把这些偏看成是高斯白噪声。好了，现在对于某一分钟我们有两个有关该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，定是23度，同时该值的高斯噪声的偏是5度（5是这样得到的：如果k-1时刻估算出的温度值的偏是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。

然后，你从温度计那里得到了k时刻的温度值，设是25度，同时该值的偏是4度。度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方（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）。他可以随不同的时刻而改变他自己的值。Dr Kalman的卡尔曼滤波器。涉及一些基本的概念知识，包括概率（Probability），随机变量（Random Variable），高斯或正态分配（Gaussian Distribution）等。首先，要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：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是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被设成高斯白噪声，他们的协方（covariance）分别是Q，R（这里我们设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是的信息处理器。下面我们来用他们结合他们的covariances来估算系统的化输出（类似上一节那个温度的例子）。首先我们要利用系统的过程模型，来预测下一状态的系统。设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更

新。我们用P表示covariance：P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg为卡尔曼增益(Kalman Gain)：Kg(k#include )= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止，我们已经得到了k状态下的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1)………(5)

其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5个基本公式。根据这5个公式，可以很容易的实现计算机的程序。这里举一个简单的例子来说明卡尔曼滤波器的工作过程。把房间看成一个系统，然后对这个系统建模。房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以U(k)=0。因此得出：X(k|k-1)=X(k-1|k-1) ……….. (6)

式子（2）可以改成：P另外你这么难的问题应该给点分才厚道啊！(k|k-1)=P(k-1|k-1) +Q ……… (7)

因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)

Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)

现在模拟一组测量值作为输入。设房间的真实温度为25度，模拟200个测量值，这些测量值的平均值为25度，但是加入了标准偏为几度的高斯白噪声。为了令卡尔曼滤波器开始工作，需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统的，从而使算法不能收敛。选取X(0|0)=1度，P(0|0)=10。

基于行人轨迹预测的无人驾驶汽车主动避撞的算法是什么？

ξ（n）=-E［e（n）v（n）］/c

首先要了解行人步行的意图外部噪声因素，要预测行人之后的运动轨迹，要通过行人图像信息的方式确定位置，之后传给车载传感器，要提高行车安全问题，还应该保证道路的正常通行，要预防转移的可能性，要预测的动态状态。

无人驾驶汽车的安全系统可以预判出周围的情况是否安全，还可以了解到周围行人的轨迹，对行人的轨迹进行预测，会预留出具体的时间和距离，这样可以提高安全性能。

通过传感器获取行人的位置和信息，分析其行走意图，提前对行人的we = 360/24/60/60pi/180; %地球自转角速度，弧度/s轨迹作出预判，主动避让行人。

卡尔曼滤波原理

T = 0%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%.1;

数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。

卡尔曼滤波不要求信号和噪声都是平稳过程的设条件。对于每个时刻的系统扰动和观测误（即噪声），只要对它们的统计性质作某些适当的定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

ekf ukf 观测测量误较小时 用哪个

Pdot[2] = -P[1][1]扩展卡尔曼滤波（EXTEND KALMAN FILTER, EKF）;

Unscented Kalman Filter 中文释义：无味卡尔曼滤波/无迹卡尔曼滤波/去芳香卡尔曼滤波 UKF是无味变换(UT) 和标准Kalman滤波体系的结合,与EKF（扩展卡尔曼滤波）不同,UKF是通过无味变换使非线性系统方程适用于线性设下的标准Kalman滤波体系,而不是像EKF那样,通过线性化非线性函数实现递推滤波.目标跟踪有两个理论基础,即数据关联和卡尔曼滤波技术 .由于在实际的目标跟踪中,跟踪系统的状态模型和量测模型多是非线性的,因此采用非线性滤波的方法.传统的非线性滤波的方法主要是扩展卡尔曼滤波算法( EKF) ,但是该算法存在着精度不高、稳定性、对目标机品牌型号：Redmibook Pro 15动反应迟缓等缺点.近年来,文献提出了一种非线性滤波算法- Unscented卡尔曼滤波(UnscentedKalman Filter,即UKF).它是根据Unscented变化(无味变换)和卡尔曼滤波相结合得到的一种算法.这种算法主要运用卡尔曼滤波的思想,但是在求解目标后续时刻的预测值和量测值时,则需要应用采样点来计算.UKF通过设计加权点δ,来近似表示n维目标采样点,计算这些δ点经由非线性函数的传播,通过非线性状态方程获得更新后的滤波值 ,从而实现了对目标的跟踪.UKF有效地克服了扩展卡尔曼滤波的估计精度低、稳定性的缺陷. 卡尔曼最初提出的滤波理论只适用于线性系统,Bucy,Sunahara等人提出并研究了扩展卡尔曼滤波(Extended Kalman Filter,简称EKF),将卡尔曼滤波理论进一步应用到非线性领域.EKF的基本思想是将非线性系统线性化,然后进行卡尔曼滤波,因此EKF是一种次优滤波.其后,多种二阶广义卡尔曼滤波方法的提出及应用进一步提高了卡尔曼滤波对非线性系统的估计性能.二阶滤波方法考虑了Taylo数展开的二次项,因此减少了由于线性化所引起的估计误,但大大增加了运算量,因此在实际中反而没有一阶EKF应用广泛. 在状态方程或测量方程为非线性时,通常采用扩展卡尔曼滤波(EKF).EKF对非线性函数的Taylor展开式进行一阶线性化截断,忽略其余高阶项,从而将非线性问题转化为线性,可以将卡尔曼线性滤波算法应用于非线性系统中.这样以来,解决了非线性问题.EKF虽然应用于非线性状态估计系统中已经得到了学术界认可并为人广泛使用,然而该种方法也带来了两个缺点,其一是当强非线性时EKF违背局部线性设,Taylor展开式中被忽略的高阶项带来大的误时,EKF算法可能会使滤波发散；另外,由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵,其繁琐的计算过程导致该方法实现相对困难.所以,在满足线性系统、高斯白噪声、所有随机变量服从高斯(Gaussian)分布这3个设条件时,EKF是最小方准则下的次优滤波器,其性能依赖于局部非线性度. 无味卡尔曼滤波是一种新型的滤波估计算法.UKF以UT变换为基础,摒弃了对非线性函数进行线性化的传统做法,采用卡尔曼线性滤波框架,对于一步预测方程,使用无味(UT)变换来处理均值和协方的非线性传递,就成为UKF算法.UKF是对非线性函数的概率密度分布进行近似,用一系定样本来逼近状态的后验概率密度,而不是对非线性函数进行近似,不需要求导计算Jacobian矩阵.UKF没有线性化忽略高阶项,因此非线性分布统计量的计算精度较高.基于上述优点,UKF被广泛应用于导航、目标跟踪、信号处理和神经网络学习等多个领域. 如需要更详细的资料,请留下邮箱地址.