kalman filter java_Kalman Filter算法详解

关键词：卡尔曼滤波线性最优估计迭代预测值估计值协方差矩阵状态转移方程观测方程最小均方估计预测+矫正

经典的卡尔曼滤波是一个迭代的线性的最优状态估计器。利用最小均方误差原理，可以保证状态值的估计是最优的。它只需要知道上次的状态估计值和当前的测量值，就可以预测到当前的最优估计值，适用于实际系统,共需5个方程既可以得到完成一次迭代。卡尔曼滤波器在singnal processing navigation control etc 应用广泛。

step1 ：时间更新(预测predict)

根据状态转移方程得到k时刻的预测值(先验估计值): X"(k) = AX(k-1|k-1) + Bu(k);

预测值(先验估计值)的协方差：P(k|k-1) = AP(k-1|k-1)AT+Q;

step2 ：状态更新(校正correct)

计算误差增益Kalman gain;Kk= P(k|k-1)HT[R+HP(k|k-1)HT]-1，其中[R+HP(k|k-1)HT]表示新息的协方差，Kalman gain越大，表示预测值对估计值的影响越小，即更相信测量值;

计算最优估计(后验估计):X(k|k) = X(k-1|k-1) + Kk[Z(k)-HX(k|k-1)]，其中Z(k)表示测量值,由观测方程Z(k)=HX(k)可得,这就是我们要到最优估计输出;

更新最有估计误差矩阵(后验估计协方差):P(k|k) = [I-KkH]P(k|k-1)

下面是Kalman filter 仿真程序 using matlab

clear all

close all

clc

A=1;

H=1;

x(1)=0;%初始状态

p(1)=1;%后验协方差估计

w1=randn(1,10000);%加速度计白噪声

t=[0.01:0.01:100];

z=sin(10*t)+w1;%加速度计测量值

R=20;%测量协方差矩阵

Q=1;%过程状态协方差矩阵for k=1:9999 %卡尔曼滤波器滤波过程

p_(k)=A*p(k)*A'+Q;

K(k)=p_(k)*H'*inv(H*p_(k)*H'+R);

x(k+1)=A*x(k)+K(k)*(z(k)-H*A*x(k));

p(k+1)=(1-K(k)*H)*p_(k);

end

figure(1),clf

plot(t,z,'b')

grid on

axis([3,5,-6,6])

title('仿真加速度计的输出')

xlabel('时间(秒)'),ylabel('仿真加速度计输出(度)')

figure(2),clf

plot(t,x,'b',t,sin(10*t),'r')

grid on

axis([3,5,-6,6])

title('加速度计估计')

legend('加速度计估计值','加速度计真值')

xlabel('时间(秒)'),ylabel('加速度计输出(度)')

测量值如下(含有测量造成v(k))

滤波效果如下,图中蓝色曲线为估计值

参考资料：

[1]https://en.wikipedia.org/wiki/Kalman_filter#Predict

[2]http://blog.csdn.net/xiahouzuoxin/article/details/39582483

kalman filter java_Kalman Filter算法详解相关推荐

多目标跟踪（MOT）中的卡尔曼滤波（Kalman filter）和匈牙利(Hungarian)算法详解
多目标跟踪(MOT)中的卡尔曼滤波(Kalman filter)和匈牙利(Hungarian)算法详解 1. 概览在开始具体讨论卡尔曼滤波和匈牙利算法之前,首先我们来看一下基于检测的目标跟踪算法的大 ...
html页面设置拦截器,SpringBoot拦截器Filter的使用方法详解
SpringBoot拦截器Filter的使用方法详解发布时间:2020-08-27 22:44:03 来源:脚本之家阅读:107 作者:玉天恒前言: 最新Servlet 3.0拦截器的使用 1. ...
基于多相滤波器的数字信道化算法详解(Python, Verilog代码已开源)
基于多相滤波器的数字信道化算法详解推导过程总结仿真本文详细介绍了基于多相滤波器的数字信道化算法的推导过程, 如果您在阅读的过程中发现算法推导过程中有任何错误, 请不吝指出. 此外, 进入我的G ...
SF图像滤镜/美颜/美妆算法详解与实战
本专栏将结合本热多年相关经验,从传统算法到火热的AI算法,给大家详细讲解目前在PC图像软件.手机图像处理类应用app,以及视频直播等应用类型中,图像视频的滤镜特效,人像美颜美妆特效的算法理论,并结合具 ...
Apollo6.0代码Lattice算法详解——Part6:轨迹评估及碰撞检测对象构建
Apollo6.0代码Lattice算法详解--Part6:轨迹评估及碰撞检测对象构建 0.前置知识 1.涉及主要函数 2.函数关系 3.部分函数代码详解 3.1 lattice_planner.cc ...
点云配准NDT (P2D)算法详解
点云配准NDT (P2D)算法详解最近了解了一些关于点云配准算法NDT的相关文章,进行总结一下. NDT算法的关键是其利用正态分布对参考点云进行了重新表示,使用点云在一个模型特定位置的似然值而不是直 ...
Deformable ConvNet算法详解（对象尺寸/形状变换）
Deformable ConvNet算法详解(DCN) 算法背景算法细节 Deformable convolution Deformable RoI pooling Deformable ConvN ...
YOLOv4算法详解
YOLOv4: Optimal Speed and Accuracy of Object Detection-论文链接-代码链接目录 1.需求解读 2.YOLOv4算法简介 3.YOLOv4算法详解 ...
敏感词或关键词过滤,DFA算法详解及python代码实现
一.前言近期项目有了一个过滤敏感词的功能需求,在网上找了一些方法及解说,发现DFA算法比较好用,容易实现,但很多文章解释得不太清楚,这里将其详细描述,并用python代码实现. 二.DFA算法详解 ...
Matlab人脸检测算法详解
这是一个Matlab人脸检测算法详解前言人脸检测结果算法详解源代码解析所调用函数解析 bwlabel(BW,n) regionprops rectangle 总结前言目前主流的人脸检测与 ...

kalman filter java_Kalman Filter算法详解

kalman filter java_Kalman Filter算法详解相关推荐

最新文章

热门文章