任意线性系统离散卡尔曼滤波器动态系统模型的推导

$$$$ display $$ \ mathbf {x} _ {k | k-1} = F \ hat {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ display $$

$$$$显示$$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$显示$$

$$$$显示$$ K = PH ^ T（HP_ {k | k-1} H ^ T + R）^ {-1} $$显示$$

$$$$ display $$ \ hat {\ mathbf {x}} _ {k} = x_ {k | k-1} + K（H \ mathbf {z} _k- \ mathbf {x} _ {k | k-1 }）$$显示$$

$$$$显示$$ P_k =（E-KH）P_ {k | k-1} $$显示$$

$$$$ display $$ \ ddot {x} =-\ omega ^ 2 x $$ display $$

$$$$显示$$ \开始{aligned} \点{x}＆= x_1 \\ \点{x} _1＆=-\ omega ^ 2 x \ end {aligned} $$显示$$

$$$$ display $$ A = \开始{bmatrix} 0＆1 \\-\ omega ^ 2＆0 \ end {bmatrix} $$ display $$

 \left[\begin{matrix}1 & 0 & 0 & T & - \frac{T^{2} w_{z}}{2} & \frac{T^{2} w_{y}}{2}\\0 & 1 & 0 & \frac{T^{2} w_{z}}{2} & T & - \frac{T^{2} w_{x}}{2}\\0 & 0 & 1 & - \frac{T^{2} w_{y}}{2} & \frac{T^{2} w_{x}}{2} & T\\0 & 0 & 0 & \frac{T^{2} \left(- w_{y}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{x} w_{y}}{2} - T w_{z} & \frac{T^{2} w_{x} w_{z}}{2} + T w_{y}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{y}}{2} + T w_{z} & \frac{T^{2} \left(- w_{x}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{y} w_{z}}{2} - T w_{x}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{z}}{2} - T w_{y} & \frac{T^{2} w_{y} w_{z}}{2} + T w_{x} & \frac{T^{2} \left(- w_{x}^{2} - w_{y}^{2}\right)}{2} + 1\end{matrix}\right] 

$$$$显示$$ \左[\开始{矩阵} 0＆0＆0＆-w_ {y} ^ {2}-w_ {z} ^ {2}＆w_ {x} w_ {y}＆w_ {x } w_ {z} \\ 0＆0＆0＆w_ {x} w_ {y}＆-w_ {x} ^ {2}-w_ {z} ^ {2}＆w_ {y} w_ {z} \ \ 0＆0＆0＆w_ {x} w_ {z}＆w_ {y} w_ {z}＆-w_ {x} ^ {2}-w_ {y} ^ {2} \\ 0＆0＆0 ＆0＆w_ {x} ^ {2} w_ {z}-w_ {z} \左（-w_ {y} ^ {2}-w_ {z} ^ {2} \右）＆-w_ {x} ^ {2} w_ {y} + w_ {y} \左（-w_ {y} ^ {2}-w_ {z} ^ {2} \右）\\ 0＆0＆0＆-w_ {y} ^ {2} w_ {z} + w_ {z} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \右）＆0＆w_ {x} w_ {y} ^ { 2}-w_ {x} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \右）\\ 0＆0＆0＆w_ {y} w_ {z} ^ {2 }-w_ {y} \左（-w_ {x} ^ {2}-w_ {y} ^ {2} \右）＆-w_ {x} w_ {z} ^ {2} + w_ {x} \左（-w_ {x} ^ {2}-w_ {y} ^ {2} \ right）＆0 \ end {matrix} \ right] $$显示$$

$$$$ display $$ \ left [\ begin {matrix} 0＆0＆0＆0＆w_ {x} ^ {2} w_ {z}-w_ {z} \ left（-w_ {y} ^ {2} -w_ {z} ^ {2} \右）＆-w_ {x} ^ {2} w_ {y} + w_ {y} \左（-w_ {y} ^ {2}-w_ {z} ^ { 2} \右）\\ 0＆0＆0＆-w_ {y} ^ {2} w_ {z} + w_ {z} \左（-w_ {x} ^ {2}-w_ {z} ^ { 2} \右）＆0＆w_ {x} w_ {y} ^ {2}-w_ {x} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \右）\ \ 0＆0＆0＆w_ {y} w_ {z} ^ {2}-w_ {y} \左（-w_ {x} ^ {2}-w_ {y} ^ {2} \右）＆- w_ {x} w_ {z} ^ {2} + w_ {x} \左（-w_ {x} ^ {2}-w_ {y} ^ {2} \右）＆0 \\ 0＆0＆0 ＆-w_ {y} \左（-w_ {x} ^ {2} w_ {y} + w_ {y} \左（-w_ {y} ^ {2}-w_ {z} ^ {2} \右）\ right）+ w_ {z} \ left（w_ {x} ^ {2} w_ {z}-w_ {z} \ left（-w_ {y} ^ {2}-w_ {z} ^ {2} \ right）\ right）＆w_ {x} \ left（-w_ {x} ^ {2} w_ {y} + w_ {y} \ left（-w_ {y} ^ {2}-w_ {z} ^ {2} \右）\右）＆-w_ {x} \左（w_ {x} ^ {2} w_ {z}-w_ {z} \左（-w_ {y} ^ {2}-w_ { z} ^ {2} \右）\右）\\ 0＆0＆0＆-w_ {y} \左（w_ {x} w_ {y} ^ {2}-w_ {x} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \ right）\ right）＆w_ {x} \ left（w_ {x} w_ {y} ^ {2}-w_ {x} \ left（ -w_ {x} ^ {2}-w_ {z} ^ {2} \ right）\ right）-w_ {z} \ left（-w _ {y} ^ {2} w_ {z} + w_ {z} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \右）\右）＆w_ {y} \左（-w_ {y} ^ {2} w_ {z} + w_ {z} \左（-w_ {x} ^ {2}-w_ {z} ^ {2} \右）\右）\\ 0 ＆0＆0＆w_ {z} \左（-w_ {x} w_ {z} ^ {2} + w_ {x} \左（-w_ {x} ^ {2}-w_ {y} ^ {2 } \右）\右）＆-w_ {z} \左（w_ {y} w_ {z} ^ {2}-w_ {y} \左（-w_ {x} ^ {2}-w_ {y} ^ {2} \右）\右）＆-w_ {x} \左（-w_ {x} w_ {z} ^ {2} + w_ {x} \左（-w_ {x} ^ {2}- w_ {y} ^ {2} \右）\右）+ w_ {y} \左（w_ {y} w_ {z} ^ {2}-w_ {y} \左（-w_ {x} ^ {2 }-w_ {y} ^ {2} \ right）\ right）\ end {matrix} \ right] $$显示$$

 import numpy as np from numpy import pi T = 1 wx, wy, wz = 0, 2*pi/100/2**.5, 2*pi/100/2**.5 vx0 = 10 A = np.array([ [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -wz, wy], [0, 0, 0, wz, 0, -wx], [0, 0, 0, -wy, wx, 0] ]) F = np.eye(6) + A * T + A @ A * T**2/2 + A @ A @ A * T**3/6 X = np.zeros((6, 101)) X[:, 0] = np.array([0, 0, 0, vx0, 0, 0]) for k in range(X.shape[1] - 1): X[:, k + 1] = F @ X[:, k] import matplotlib as mpl import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(X[0, :], X[1, :], X[2, :]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.show()