无人驾驶技术——初探Kalman滤波器
2024-05-09 12:39:53
文章目录
- 高斯分布
- 高斯公式
- 将两个Gaussian相乘
- 计算新的高斯的均值和方差
- 卡尔曼滤波器
- 预测函数
- 一维kalman 实现
- 多维卡尔曼滤波器
- 多维高斯分布 或者 多元高斯
- 多维卡尔曼滤波器
- 多维 Kalman 实现
- C++实现Kalman滤波（一维追踪问题）
Kalman滤波器在物体跟踪，机器人和无人驾驶领域有很重要的作用，这篇文章主要是从实战方面对Kalman滤波器学习的一个总结和记录。
卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！
高斯分布
高斯公式
from math import *def f_gaussian(mu, sigma2, x):return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)print (f_gaussian(10.,4.,2.)) # 当将2也为10时，能获取最大的函数值
6.691511288244268e-05
将两个Gaussian相乘
delta^2 越小，曲线形状越陡峭
计算新的高斯的均值和方差
# Write a program to update your mean and variance
# when given the mean and variance of your belief
# and the mean and variance of your measurement.
# This program will update the parameters of your
# belief function.def update(mean1, var1, mean2, var2):new_mean = (1/(var1 + var2)) * (var2*mean1 + var1*mean2)new_var = 1 / (1/var1 + 1/var2)return [new_mean, new_var]print (update(10.,8.,13., 2.))
[12.4, 1.6]
卡尔曼滤波器
卡尔曼滤波器分为测量和预测两步，其中测量部分就是我们上部分的高斯函数的update部分，而预测主要是靠加法就可以完成
预测函数
# Write a program that will predict your new mean
# and variance given the mean and variance of your
# prior belief and the mean and variance of your
# motion. def update(mean1, var1, mean2, var2):new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)new_var = 1/(1/var1 + 1/var2)return [new_mean, new_var]def predict(mean1, var1, mean2, var2):new_mean = mean1 + mean2new_var = var1 + var2return [new_mean, new_var]print (predict(10., 4., 12., 4.))
[22.0, 8.0]
一维kalman 实现
一维Kalman滤波器
# Write a program that will iteratively update and
# predict based on the location measurements
# and inferred motions shown below. def update(mean1, var1, mean2, var2):new_mean = float(var2 * mean1 + var1 * mean2) / (var1 + var2)new_var = 1./(1./var1 + 1./var2)return [new_mean, new_var]def predict(mean1, var1, mean2, var2):new_mean = mean1 + mean2new_var = var1 + var2return [new_mean, new_var]measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2.
mu = 0.
sig = 10000.
#sig = 0.0000000001#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig]. # Insert code here
for i in range(len(measurements)):[mu,sig] = update(mu,sig, measurements[i], measurement_sig)print ('update: ',[mu, sig])[mu,sig] = predict(mu,sig, motion[i], motion_sig)print ('predict: ',[mu, sig])
print ('the result: ',[mu, sig])update: [4.998000799680128, 3.9984006397441023]
predict: [5.998000799680128, 5.998400639744102]
update: [5.999200191953932, 2.399744061425258]
predict: [6.999200191953932, 4.399744061425258]
update: [6.999619127420922, 2.0951800575117594]
predict: [8.999619127420921, 4.09518005751176]
update: [8.999811802788143, 2.0235152416216957]
predict: [9.999811802788143, 4.023515241621696]
update: [9.999906177177365, 2.0058615808441944]
predict: [10.999906177177365, 4.005861580844194]
the result: [10.999906177177365, 4.005861580844194]
多维卡尔曼滤波器
多维高斯分布 或者 多元高斯
多维卡尔曼滤波器
多维 Kalman 实现
# Write a function 'kalman_filter' that implements a multi-
# dimensional Kalman Filter for the example givenfrom math import *class matrix:# implements basic operations of a matrix classdef __init__(self, value):self.value = valueself.dimx = len(value)self.dimy = len(value[0])if value == [[]]:self.dimx = 0def zero(self, dimx, dimy):# check if valid dimensionsif dimx < 1 or dimy < 1:raise ValueError("Invalid size of matrix")else:self.dimx = dimxself.dimy = dimyself.value = [[0 for row in range(dimy)] for col in range(dimx)]def identity(self, dim):# check if valid dimensionif dim < 1:raise ValueError("Invalid size of matrix")else:self.dimx = dimself.dimy = dimself.value = [[0 for row in range(dim)] for col in range(dim)]for i in range(dim):self.value[i][i] = 1def show(self):for i in range(self.dimx):print(self.value[i])print(' ')def __add__(self, other):# check if correct dimensionsif self.dimx != other.dimx or self.dimy != other.dimy:raise ValueError("Matrices must be of equal dimensions to add")else:# add if correct dimensionsres = matrix([[]])res.zero(self.dimx, self.dimy)for i in range(self.dimx):for j in range(self.dimy):res.value[i][j] = self.value[i][j] + other.value[i][j]return resdef __sub__(self, other):# check if correct dimensionsif self.dimx != other.dimx or self.dimy != other.dimy:raise ValueError("Matrices must be of equal dimensions to subtract")else:# subtract if correct dimensionsres = matrix([[]])res.zero(self.dimx, self.dimy)for i in range(self.dimx):for j in range(self.dimy):res.value[i][j] = self.value[i][j] - other.value[i][j]return resdef __mul__(self, other):# check if correct dimensionsif self.dimy != other.dimx:raise ValueError("Matrices must be m*n and n*p to multiply")else:# multiply if correct dimensionsres = matrix([[]])res.zero(self.dimx, other.dimy)for i in range(self.dimx):for j in range(other.dimy):for k in range(self.dimy):res.value[i][j] += self.value[i][k] * other.value[k][j]return resdef transpose(self):# compute transposeres = matrix([[]])res.zero(self.dimy, self.dimx)for i in range(self.dimx):for j in range(self.dimy):res.value[j][i] = self.value[i][j]return res# Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functionsdef Cholesky(self, ztol=1.0e-5):# Computes the upper triangular Cholesky factorization of# a positive definite matrix.res = matrix([[]])res.zero(self.dimx, self.dimx)for i in range(self.dimx):S = sum([(res.value[k][i])**2 for k in range(i)])d = self.value[i][i] - Sif abs(d) < ztol:res.value[i][i] = 0.0else:if d < 0.0:raise ValueError("Matrix not positive-definite")res.value[i][i] = sqrt(d)for j in range(i+1, self.dimx):S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])if abs(S) < ztol:S = 0.0try:res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]except:raise ValueError("Zero diagonal")return resdef CholeskyInverse(self):# Computes inverse of matrix given its Cholesky upper Triangular# decomposition of matrix.res = matrix([[]])res.zero(self.dimx, self.dimx)# Backward step for inverse.for j in reversed(range(self.dimx)):tjj = self.value[j][j]S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])res.value[j][j] = 1.0/tjj**2 - S/tjjfor i in reversed(range(j)):res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]return resdef inverse(self):aux = self.Cholesky()res = aux.CholeskyInverse()return resdef __repr__(self):return repr(self.value)######################################### Implement the filter function belowdef kalman_filter(x, P):for n in range(len(measurements)):# measurement updateZ = matrix([[measurements[n]]])y = Z-(H*x)S = H*P*H.transpose()+RK = P*H.transpose()*S.inverse()x = x +(K*y)P = (I-(K*H))*P# predictionx = (F*x)+uP = F*P*F.transpose()return x,P############################################
### use the code below to test your filter!
############################################measurements = [1, 2, 3]x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrixprint(kalman_filter(x, P))
# output should be:
# x: [[3.9996664447958645], [0.9999998335552873]]
# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]([[3.9996664447958645], [0.9999998335552873]], [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]])
C++实现Kalman滤波（一维追踪问题）
/** * Write a function 'filter()' that implements a multi-* dimensional Kalman Filter for the example given*/#include <iostream>
#include <vector>
#include "Dense"using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;// Kalman Filter variables
VectorXd x; // object state
MatrixXd P; // object covariance matrix
VectorXd u; // external motion
MatrixXd F; // state transition matrix
MatrixXd H; // measurement matrix
MatrixXd R; // measurement covariance matrix
MatrixXd I; // Identity matrix
MatrixXd Q; // process covariance matrixvector<VectorXd> measurements;
void filter(VectorXd &x, MatrixXd &P);int main() {/*** Code used as example to work with Eigen matrices*/// design the KF with 1D motionx = VectorXd(2);x << 0, 0;P = MatrixXd(2, 2);P << 1000, 0, 0, 1000;u = VectorXd(2);u << 0, 0;F = MatrixXd(2, 2);F << 1, 1, 0, 1;H = MatrixXd(1, 2);H << 1, 0;R = MatrixXd(1, 1);R << 1;I = MatrixXd::Identity(2, 2);Q = MatrixXd(2, 2);Q << 0, 0, 0, 0;// create a list of measurementsVectorXd single_meas(1);single_meas << 1;measurements.push_back(single_meas);single_meas << 2;measurements.push_back(single_meas);single_meas << 3;measurements.push_back(single_meas);// call Kalman filter algorithmfilter(x, P);return 0;
}void filter(VectorXd &x, MatrixXd &P) {for (unsigned int n = 0; n < measurements.size(); ++n) {VectorXd z = measurements[n];// TODO: YOUR CODE HEREVectorXd y = z - H * x;MatrixXd Ht = H.transpose();MatrixXd S = H * P * Ht + R;MatrixXd Si = S.inverse();MatrixXd K = P * Ht * Si;// new statex = x + (K * y);P = (I - K * H) * P;/*** KF Prediction step*/x = F * x + u;MatrixXd Ft = F.transpose();P = F * P * Ft + Q;cout << "x=" << endl << x << endl;cout << "P=" << endl << P << endl;}
}
无人驾驶技术——初探Kalman滤波器相关推荐
- 无人驾驶技术——无损卡尔曼滤波（UKF）
文章目录 无损卡尔曼滤波(UKF) UKF Predict 1.选择预测点sigma点 sigma点计算代码实现 添加过程噪声处理 构建噪声扩充矩阵 c++ 2.预测sigma点 预测sigma点方法 ...
- 无人驾驶系列】光学雷达（LiDAR）在无人驾驶技术中的应用
无人驾驶汽车的成功涉及高精地图.实时定位以及障碍物检测等多项技术,而这些技术都离不开光学雷达(LiDAR).本文将深入解析光学雷达是如何被广泛应用到无人车的各项技术中.文章首先介绍光学雷达的工作原理, ...
- 【智能驾驶】最全、最强的无人驾驶技术学习路线
作者:许小岩 来源:AI脑力波 授权 产业智能官 转载. 近两年,国内外掀起了一场空前的无人驾驶热潮.特斯拉.谷歌.福特.奔驰.丰田.百度.滴滴等众多企业开始研发无人驾驶汽车,甚至不少企业已经计划 ...
- 【无人驾驶系列】光学雷达（LiDAR）在无人驾驶技术中的应用
作者:刘博聪,刘少山,James Peng 责编:周建丁(投稿请联系zhoujd@csdn.net) 声明:<程序员>原创文章未经允许不得转载,更多精彩文章请订阅2016年程序员:http ...
- 【无人驾驶二】光学雷达（LiDAR）在无人驾驶技术中的应用
作者:刘博聪,刘少山,James Peng 责编:周建丁(投稿请联系zhoujd@csdn.net) 声明:<程序员>原创文章未经允许不得转载,更多精彩文章请订阅2016年程序员:ht ...
- 无人驾驶技术--五个等级
美国高速公路交通安全委员会(NHTSA)将无人驾驶技术分成了五个等级: L0:驾驶者拥有百分之百的控制权,车辆没有任何安全系统辅助设备,目前绝大部分车辆属此层级; L1:车辆拥有单个或多个独立功能电子 ...
- 哥伦比亚大学AI实验室主任Hod Lipson：阻碍无人驾驶技术发展的7个误区
来源:智车科技 摘要:我们发现有些针对无人驾驶的误解还在广泛肆意传播,并且这些信息会被反对者拿来和对抗无人驾驶的推广政策. 每年,全世界都有将近120万人死于车祸,这个死亡率相当于每年释放10个广岛级 ...
- 无人驾驶技术排名：百度居中游，苹果特斯拉垫底 | 行业
来源:网易科技 概要:其调查研究显示,无人驾驶汽车行业的现状已经发生了天翻地覆的变化,而特斯拉.苹果公司在今年的排名垫底. 1月17日消息,据CNET网站报道,日前,美国市场研究机构Navigant ...
- matlab疲劳驾驶_第一本无人驾驶技术书
第一本无人驾驶技术书(第2版) 1.<第一本无人驾驶技术书(第2版)> 刘少山,唐洁,吴双,李力耘 等 著,2019-09-01, 电子工业出版社 适读人群 :本书适合人工智能.深度学习. ...
最新文章
- 诺贝尔物理学奖得主Arthur Ashkin去世，他发明了“激光镊子”，曾抱怨被诺奖遗忘...
- KMM+Compose 开发一个Kotlin多平台应用
- PAT_B_1006 换个格式输出整数
- 拾遗：『ext4 Quota』
- kotlin spring-webflux netty
- STM32 ISP 下载程序， C源码，
- Ubuntu18.04.3虚拟机安装步骤图文教程
- 2021牛客第一场 K.Knowledge Test about Match
- OpenCV显示中文汉字，未使用CvxText和FreeType库
- (hdu step 8.1.1)ACboy needs your help again!(STL中栈和队列的基本使用)
- python如何搜索关键字_Python遍历目录和搜索文件中的关键字
- asp.net core IIS发布
- 常规对数据库库的操作
- I2C总线协议原理介绍
- AcrelEMS-BP生物制药工厂能效管理系统
- PLC的IO点位是什么意思
- 黄金分割点（java）
- Mixed supervision for surface-defect detection: from weakly to fully supervised learning：表面缺陷检测的混合监督
- 那个人路过了青春一阵子，却会在记忆里搁浅一辈子
- 关于软件测试的论文文库,软件测试毕业论文[共46页]