最近简单看了一下kalman滤波,这里先写些笔记
卡尔曼滤波说的是“两套系统“得到的计算量或者测量量,一个是根据系统状态预测出来的下一个状态的各种参量,另一个是实际测量值,那么我们怎么在这两套系统中找到更合适的值。
说白了,它所谓的这两套系统都是服从高斯分布的,他们的乘也符合高斯分布,根据这两个高斯分布的方差的大小,调整到合适的方差选择
F有的地方有人喜欢写成A。B是控制矩阵, x应该加的是均值为零的噪声,所以就省略掉没写,B有的文献喜欢写成C(control),P,协方差矩阵,有的地方也喜欢写成V的
H是观测矩阵,是你最后的观测值和待分析状态之间的矩阵转移关系,有地地方喜欢写成C,比如 我只能观测到所有状态的和,那么,测量值(z) = [1 1 1 1 1...] * [ x1 x2 x2....]T ,R 就是1*1维度的,另外 y 就是 测量余量(误差),注意,y 和 x 没什么自变量应变量之间的关系,别看到x,y就想到 y =f(x)
x 是某些状态值(不一定能观察到,或者是某些变量的中间传递函数),有点隐含状态的意思
kalman描述的是一些原始轨迹或者规律不方便描述,但有状态之间的转移是了解的(A矩阵),另外 H 是一个转换矩阵
它能 H * x(隐含状态) = 观测值
p0 和Q 都可以先设为对角矩阵
Q 和 R的设定,记住,Q/(Q+R) 就是卡尔曼增益的收敛值
Q越大,说明测量值可靠,R越大,说明预测值越可靠
X的初始值和方差可以简单设置,后面会收敛,难的是Q和R的值
扩展Extended kalman filter
g函数是状态转移函数了,不需要A矩阵了,参与运算的Gt在计算方差的时候出现
h函数是观测函数了,Ht是H函数的求偏导
记忆公式我也不擅长,我找了两个工具箱,分别是kalman 和扩展kalman 的代码
做了一些改动,上传至:
https://download.csdn.net/download/book_bbyuan/11729042
我reference的是一个很好的 KF和 EKF的例子
http://www.pyrunner.com/weblog/2018/04/12/kalman-example/
我按它给出的python的函数,自己适当改动了一下,使用matlab 改写,下图是我做的实验
原问题描述的是做8字运行的小车的轨迹,下三图分别是 x y位置,角速度,速度的真实状态和含噪数据
首先使用kalman滤波器,我用的是
https://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html
中剥离出来,它实际就三个函数是核心的,kalman_filter.m kalman_update.m kalman_filter.m, 我直接杂糅到一个函数里面了
状态:四个: x dx/dt dx2/dt2 y dy/dt dy2/dt2 ,,x y是位置
观测值:两个, x_gps y_gps
得到结论如下: 黑色是实际运行轨迹,蓝色点是观测值,橙红色是KF滤波之后结果
下面尝试使用扩展卡尔曼滤波器
这时候状态值还是 上面说的四元组
但观测值,这次是四个:
使用的扩展卡尔曼滤波器,reference这里:
https://www.cbcity.de/das-extended-kalman-filter-einfach-erklaert
https://www.mathworks.com/matlabcentral/fileexchange/18189-learning-the-extended-kalman-filter
原公式需要求各个状态值的偏导数
jacobi行列式的原来定义
https://www.codetd.com/article/2131570 有一个例子
他的特点是不用记公式,只要描述x中的不同状态变量之间的关系就可以
求偏导的事情交给工具就行了
黑色是实际运行轨迹,蓝色点是观测值,橙红色是KF滤波之后结果
扩展的效果要优于一般的kalman滤波器
代码上传到: