問題：通過感測器可以測量出自由落體時的加速度、速度和位置，通過卡爾曼濾波估計速度和位置。
座標系：向下為正，初始位置和速度都為0
先上卡爾曼濾波公式：
然後上**：
# -*- coding: utf-8 -*
# 向下為正方向
import numpy as np
import matplotlib.pyplot as plt
def main():
# 時間共1s，取樣週期10ms
dt = 0.01
t = [i * dt for i in range(0, 100)]
g = 9.8
# 真實值
x_true_mat = np.mat(0.5 * g * np.multiply(np.array(t), np.array(t)))
v_true_mat = g * np.mat(t)
u_true_mat = np.mat([g for i in range(0, 100)])
# 雜訊
x_noise = np.round(np.random.normal(0, 0.1, 100), 2)
v_noise = np.round(np.random.normal(0, 0.1, 100), 2)
u_noise = np.round(np.random.normal(0, 0.01, 100), 2)
x_noise_mat = np.mat(x_noise)
v_noise_mat = np.mat(v_noise)
u_noise_mat = np.mat(u_noise)
# 測量值
x_z_mat = x_true_mat + x_noise_mat
v_z_mat = v_true_mat + v_noise_mat
u_mat = u_true_mat + u_noise_mat
# 定義x的初始狀態
x_mat = np.mat([[0], [0]])
# 定義初始狀態協方差矩陣
p_mat = np.mat([[1, 0], [0, 1]])
# 狀態轉移矩陣
f_mat = np.mat([[1, dt], [0, 1]])
# 控制矩陣
b_mat = np.mat([[0.5 * dt * dt], [dt]])
# 定義狀態轉移協方差矩陣，這裡我們把協方差設定的很小，因為覺得狀態轉移矩陣準確度高
q_mat = np.mat([[1.0 * 1.0 * dt * dt, 0], [0, 1.0 * 1.0 * dt * dt]])
# 定義觀測矩陣
h_mat = np.mat([[1, 0], [0, 1]])
# 定義觀測雜訊協方差
r_mat = np.mat([[1.0 * 1.0, 0], [0, 2.5 * 2.5]])
for i in range(100):
x_predict = f_mat * x_mat + b_mat * u_mat[0, i]
p_predict = f_mat * p_mat * f_mat.t + q_mat
k = p_predict * h_mat.t * (h_mat * p_predict * h_mat.t + r_mat).i
zt = np.mat([[x_z_mat[0, i]], [v_z_mat[0, i]]])
x_mat = x_predict + k * (zt - h_mat * x_predict)
p_mat = (p_mat - k * h_mat) * p_predict
plt.plot(t[i], x_z_mat[0, i], 'ro', markersize=1)
plt.plot(t[i], v_z_mat[0, i], 'ro', markersize=1)
plt.plot(t[i], x_mat[0, 0], 'bo', markersize=1)
plt.plot(t[i], x_mat[1, 0], 'bo', markersize=1)
plt.show()
if __name__ == '__main__':
main()
圖中的紅色點分別是觀測的位置和速度，藍色點為估計出的位置和速度。
卡爾曼 卡爾曼濾波 1
今天主要介紹一下卡爾曼濾波器，所謂卡爾曼濾波器其實是一種最優化遞迴數字處理演算法 optimal recursive data processing algorithm 卡爾曼濾波器應用 既然我們有了測量儀器，這些測量儀器可以目標給出準確測量值。還需要卡爾曼濾波器進行估計嗎?下面解釋一下為什麼需要卡...
卡爾曼 基礎卡爾曼濾波
卡爾曼濾波器是一種基礎 定位演算法。原理非常簡單易懂。核心過程可以用乙個圖說明 本質上就是這兩個狀態過程的迭代，來逐步的準確定位。更新 更具感測器獲取到比較準確的位置資訊後來更新當前的 問位置，也就是糾正 的錯誤。你可能要問為什麼有感測器的資料了還要進行更新？因為在現實世界中感測器是存在很多雜訊干擾...
卡爾曼濾波
卡爾曼濾波演算法 首先引入乙個離散控制過程的系統，用乙個線性隨機微分方程來描述 x k a x k 1 b u k w k 系統的測量值 z k h x k v k x k 是k時刻的系統狀態，u k 是k時刻對系統的控制量。a和b是系統引數，對於多模型系統，他們為矩陣。z k 是k時刻的測量值，h...