基础卡尔曼定位-多维情况

之前在基础kalman介绍了一维的情况,但是在现实世界中,是很少存在一维情况的,比如一辆自动驾驶汽车停在马路上,它的当前状态是由 [公式] 这样的思维坐标表示的。x,y,z是三维坐标值, [公式] 是方向。更有一种情况是 [公式] ,x表示的位置(类似一维坐标中的坐标位置), [公式] 代表的是速度。这种情况很适合分析介绍多维卡尔曼的应用情况和分析其优势。下面我们就这个位置速度的2维状态进行介绍。

kalman是建立在线性计算基础上并且符合高斯分布的预测评估方法。所以先介绍下载多维情况下的高斯分布。

图一:这是初始的车辆状态,可以从高斯图中看出,速度我们不确定,因为没有传感器测量告诉我们,但是当前的位置在10.

图二:

预测:现在我们的预测函数是位置加速度的线性计算,假设 [公式] ==1. 我们可以发现速度在取不同值得时候位置位置的分布如红色的高斯所示。

更新:这时候我们获取到了下个时刻的传感器值,显示当前的x位置在14。传感器也有误差,并且不能检测v,所以我们看到在14的地方高斯分布。

神奇的地方出现了,有没有注意到更新高斯和预测的高斯交汇了,这个交汇点就是更新值。可以看到v现在更新值为4.但是还是存在一定误差的所以方差会比较大。

就这样不停的进行迭代就能获取到比较准确的多维值。

卡尔曼滤波的一大优点就是可以预测所谓的隐藏值。比如这里无法进行测量的速度参数。

实现公式: