卡尔曼算法详解(C++程序)

在记住卡尔曼抽出器垄断,率先,看一眼为什么。卡尔曼。和安宁著名的大众化的观念(如Fu Liye交替的),泰勒集合等。,卡尔曼亦一的名字,与他们不两者都的是,他是一点钟当代人主义者。!

卡尔曼全名Rudolf Emil Kalman,匈牙利数学家,1930出生于匈牙利布达佩斯。19531954从丰盛的上接到电机工程单身汉和硕士学位。1957他于2000在哥伦比亚特区综合性大学接到博士学位。。我们家现时要记住的卡尔曼抽出器,这是从他的博士论文和19602003宣布论文A
New Approach to Linear Filtering and Prediction Problems
(一种新的一次的滤波和预测方式)。设想你对这篇论文感兴趣,,你可以在在这里下载地址。:

简略来说,卡尔曼抽出器是一点钟“optimal recursive data processing algorithm(优选法的自回归资料处置算法)。团成绩都处置了。,他是最好的,生产率难以置信的,甚至最可供应用的。。他的普及的勤勉早已超越30年,包罗似人自动机航行,把持,搭车知调停,甚至在军用无线电探测器体系和导弹拥护者等枝节的。晚近,它已被勤勉于电脑图像处置。,人脸歧视,拿 … 来说,图像粉碎,图像端检测等。。

2.卡尔曼抽出器的绍介
Introduction to the Kalman Filter

为了可以一切的轻易的包含卡尔曼抽出器,在在这里,我们家将应用图像表现方式来解说。,代表详表丰盛的的数学表现和数学表现。不管到什么弄平,他的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=,我们家可以推断。k不时地的现实高烧为:23+*(25-23)=。可以看出,由于高烧计covariance绝对较小(置信高烧计),故,推断的冠高烧值屈身于THE。。

现时我们家自明了。k冠的时期是高烧。,下一步是进入。k+1不时地,新的最优推断。到现时为止,如同我们家无理解一点东西背部。。对了,在进入中k+1不时地垄断,我们家不得不弄清楚。k不时地指前面提到的事物最优值(弄平离去。该算法如次((1-Kg)*5^2)^。在这里的5执意下面的k你事先预测的指前面提到的事物。23高烧离去,通用的这是进入k+1顷刻以前k在一点时期推断的冠高烧值的离去(CORES)3)。

执意这么,卡尔曼抽出器就不时的把covariance递归式,可以断定出冠的高烧值。。他跑得很快。,它只保存了决定性的一瞬。covariance。下面的Kg,执意卡尔曼增益(Kalman
Gain
)。他可以在不两者都的时期互换本身的等值的。,很神奇吗?!

现时我们家开端谈正经事儿。,议论真正工程体系上的卡尔曼。

3. 卡尔曼抽出器算法
The Kalman Filter Algorithm

在这钟爱的,我们家将表现来自。Dr Kalman 的卡尔曼抽出器。以下阐明,它归结起来十足的根本的请求知。,包罗概率Probability),随后变量(Random
Variable
),高斯或正态分派(Gaussian Distribution)常State-space Model依此类推。但在流行中的卡尔曼抽出器的微不足道的使宣誓,在这里不克不及表现。。

率先,率先绍介了团圆把持航线体系。。该体系可以采用一次的随机特意的方程。Linear Stochastic Difference equation)来表现:
X(k)=A X(k-1)+B U(k)+W(k) 
加法体系。测值
Z(k)=H X(k)+V(k) 
决定性的两个表现,X(k)k体系时期身份,U(k)k天天把持体系。AB是体系参量,多图案体系,它们是矩阵。。Z(k)k不时地的测值,H它是测体系的参量。,多测体系,H为矩阵。W(k)V(k)使著名表现航线和测噪声。。它们被补助金为高斯白噪声。(White
Gaussian 噪声)
,他们的covariance分莫非QR(我们家假说它们不随体系的身份而使多样化)。

满足的是你这么说的嘛!环境(一次的随机特意的体系,航线和测是高斯白噪声。),卡尔曼抽出器是最优的传达处置器。让我们家把它们混合起来。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)对应的covarianceP(K-1)K-1X(K-1×K-1)对应的covarianceA’表现A的转置矩阵,Q这是一点钟体系的航线。covariance。式子12执意卡尔曼抽出器5前两个表现。,执意说说,体系的预测。。

现时我们家接到了介绍身份的预测出路。,事先的,我们家搜集的测值的介绍身份。。预测与测值的结成,我们家可以接到介绍身份。(k)最优推断值X(k|k)
X(k|k)= K(K 1) kg(K) (Z(k)-H X(k|k-1)) ……… (3)
内侧的Kg为卡尔曼增益(Kalman 增益)
Kg(k) 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) HP(k|k-1) ……… (5)
内侧的1的矩阵,单图案单次测,I=1。当体系进入k+1身份身份,P(k|k)这执意作风(2)P(K-1)K-1。这么,该算法可以自回归管理。。

卡尔曼抽出器的规律根本表现了,式子12345执意他的5个根本表现。本着这5个表现,很轻易赚得电脑顺序。。

下面,我将用这事顺序作出一点钟现实管理的使习惯于。。。。

4. 简略使习惯于
A Simple Example

在这里我们家混合次货个第三链杆。,举一点钟十足的简略的使习惯于来阐明卡尔曼抽出器的任务航线。该示例是对次货党派的更进一步表现。,它也将婚配顺序虚构的的出路。。

本着次货党派的表现,把房间设想成一点钟体系。,事先的对体系停止建模。。自然,我们家所理解的图案不需求十足的精密。。我们家所晓得的这事房间的高烧是靠近一时期高烧两者都的,因而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。式子345可以更反而以下:
X(k|k)= K(K 1) kg(K) (Z(k)-X(k|k-1)) ……… (8)
Kg(k) P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=1-Kg(k)P(k|k-1) ……… (10)

现时我们家模仿一组测作为输出。。假说房间的现实高烧是25度,我模仿了它200个测值,这些测值的平均值为25度,但高斯白噪声的标准离去为几度。。

为了令卡尔曼抽出器开端任务,我们家需求告知卡尔曼两个零不时地的始值,是X(0|0)P(0|0)。他们不太取决于本身的等值的观。,把它给一点钟。,由于跟随卡尔曼的任务,X会逐步收敛。。不管到什么弄平在流行中的P,普通不采用。0,由于这么可能性会令卡尔曼完整置信你预先布置的X(0|0)这是最好的名人。,相应地使算法不克不及收敛。。我选择了它X(0|0)=1度,P(0|0)=10

体系的现实高烧为25度,图片中应用黑线。。图中对某社团进行经济歧视是卡尔曼抽出器出口的最优选法出路(该出路在算法中设置了Q=1e-6R=1e-1)。

××××××××××××××××××

matlab下面的kalman滤波顺序:

clear
N=200;
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
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)
end

t=1:N;
以图表画出(t),s,”r”,t,Y,”g”,t,x,”b”);

Kalman 术语申述:

(1)         预测:先验推断x[nnn-1 ]=a*x[n-1αn-1 ]

一维使习惯于,A可以用作恒量。,常取1,同时,它通常需求零。:0连续重击可以吗?,我可以向你誓言。,成绩罕有地。相反地深思熟虑,这朴素地一点钟推断。,推断的噪声方差可以被改良。

(2)         前向共变:做出预测后的新的概率散布的方差(预测前番的最优推断为介绍不时地的先验推断这事航线可以当成一点钟契合预测航线噪声散布及其它(决定性的一点钟冠推断可以认为是高斯散布)。预测出路这也与高斯噪声散布分歧。,方差是两个交互孤独的方差的和。。

一维使习惯于,p[n~(n-1)]=p[n-1αn-1 ] q。 Q是预测方差。,表现对预测的猜疑。,鉴于现实监视的工程改良抽出器功能

(3)         计算卡尔曼增益:

一维使习惯于,K[n]=H*P[n=n-1 ] /{H^ 2*P[nnn-1 ] r}。H是观察力的作出反应乘数。,通常1,R是测的方差。,普通来说,工程可以连续的接到。

(4)         更多推断值:后验推断,改良后的推断值,更使移近真实等值的。

一维使习惯于,冠推断是由以下作出的。:

x[n|n]=x[n|n-1]+K[n]*{z[n]-x[n|n-1]}。Z[n]是观察力值。

(5)         花样翻新偏航共变:最优推断的概率散布的方差是OBTA。。

一维使习惯于,新的偏航共变以以下使符合作出。:

p[n[n]=(1-k[n]*h)*p[n~(n-1)]

一维抽出器传达作曲typedef  struct{double filterValue;  k-1时期的滤波值,这执意K-1时期的等值的。double kalmanGain;   //   Kalamn增益double A;   x(n)=a*x(n-1) u(n),u(n)~N(0,Q)double H;   Z(n)=h*x(n) w(n),w(n)~N(0,R)double Q;   预测航线中噪声离去的方差double R;   测噪声离去,体系使开始作用后,,经过测人口普查试验接到的double P;   推断偏航共变}  KalmanInfo;/*** @brief Init_KalmanInfo   设定初值抽出器的始值传达抽出器柄PARAM Q预测噪声方差由表面体系测。用表面体系测PARAM R的测噪声方差*/void Init_KalmanInfo(KalmanInfo* info, double Q, double R){info->A = 1;  //不变式卡尔曼info->H = 1;  //info->P = 10;  后验身份推断偏航方差的始值(NO)info->Q = Q;    预测(航线)噪声方差冲击收敛摧毁。,可以本着现实需求作出。info->R = R;    测(观察力)噪声方差可以经过试验MEA接到。info->filterValue = 0;测始值}double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement){预测再的等值的double predictValue = kalmanInfo->A* kalmanInfo->filterValue;   X的先验推断是从后验推断和输出传达导出的。,在这里我们家需求本着基数的身高做一点钟修正。//求共变kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+qdouble preValue = kalmanInfo->filterValue;  记载决定性的现实成为同等值。计算卡尔曼增益kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R);  //Kg(k) P(k|k-1) H’ / (H P(k|k-1) H’ + R)//改良出路,即计算抽出器值。kalmanInfo->filterValue = predictValue + (lastMeasurement - predictValue)*kalmanInfo->kalmanGain;  应用残差传达改良X(t)推断,作出了后验推断。,这事值执意说出口  X(k|k)= K(K 1) kg(K) (Z(k)-H X(k|k-1))花样翻新后验推断kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]return  kalmanInfo->filterValue;}

发表评论

Close Menu