写这篇文章是因为在学习卡尔曼滤波的时候发现，只有线性运动可以用卡尔曼滤波，而非线性运动需要用到扩展卡尔曼滤波（EKF）或者无迹卡尔曼滤波（UKF）。那么又发现自己不熟悉非线性运动的运动模型，所以学了一下，并且希望用python对他们进行重现。
一些参考文章：
自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导__归尘_的博客-CSDN博客_cv运动模型
滤波笔记二：无迹卡尔曼滤波 CTRV&&CTRA模型_泠山的博客-CSDN博客_ctra模型
3月16日 CV,CA,CTRV等运动模型，EKF，UKF在运动模型下的分析与实践_Hali_Botebie的博客-CSDN博客_ca运动模型
卡尔曼滤波理论讲解与应用（matlab和python）_O_MMMM_O的博客-CSDN博客_ekf卡尔曼滤波
扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）_O_MMMM_O的博客-CSDN博客_ekf python
无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波_AdamShan的博客-CSDN博客
（组会参考）无人驾驶4： 无损卡尔曼滤波_科学边界的博客-CSDN博客
本文目录：
目录
1.CV模型（Constant Velocity）恒定速度模型
2.CA模型（Constant Acceleration）恒定加速度模型
3.CTRV模型（Constant Turn Rate and Velocity 恒定转弯率和速度模型）
1.CV模型（Constant Velocity）恒定速度模型
该模型可以参考之前的文章：滤波笔记一：卡尔曼滤波（Kalman Filtering）详解_scoutee的博客-CSDN博客
该篇文章中举了一个python实现的二维CV例子（匀速直线运动）。
2.CA模型（Constant Acceleration）恒定加速度模型
根据上述的参考文献，写了如下笔记：
2.1 分析算法编程需要建立的变量
因为CA模型属于线性运动模型，所以可以直接用 卡尔曼滤波。
2.1.1 基于状态空间表达式要建立以下变量
状态空间表达式（建立的是真实值）：
变量名代码中对应值状态变量的真实值
X_true状态变量的分量X坐标的真实值position_x_trueY坐标的真实值position_y_trueX方向速度的真实值speed_x_trueY方向速度的真实值speed_y_trueX方向加速度的真实值acceleration_x_trueY方向加速度的真实值acceleration_y_true测量值的真实值测量值
Z测量值的分量（均为真实值）注意我们观察的时候，只测xy坐标两个变量X坐标的真测量值position_x_measureY坐标的真测量值position_y_measureX方向速度的真测量值speed_x_measureY方向速度的真测量值speed_y_measureX方向加速度的真测量值acceleration_x_measureY方向加速度的真测量值acceleration_y_measure噪声过程噪声
WX坐标的噪声w_position_xY坐标的噪声w_position_yX方向速度的噪声w_speed_xY方向速度的噪声w_speed_yX方向加速度的噪声w_acceleration_xY方向加速度的噪声w_acceleration_y测量噪声
VX坐标的测量噪声v_position_xY坐标的测量噪声v_position_y系数矩阵状态矩阵AA传输矩阵HH
2.1.2 基于卡尔曼滤波的五步迭代要建立以下变量
变量名代码中对应变量状态变量状态变量的先验估计值
X_prior状态变量的后验估计值
X_ posteriori协方差矩阵误差ek的协方差矩阵PkP误差ek的协方差矩阵Pk的先验P_prior过程噪声w的协方差矩阵QQ测量噪声v的协方差矩阵RR卡尔曼增益
2.1.3 分析各个变量的初始值
本例的代码参考了：卡尔曼滤波理论讲解与应用（matlab和python）_O_MMMM_O的博客-CSDN博客_ekf卡尔曼滤波
为了方便后续计算，设
不妨设初始位置
初始速度v=5,加速度a=4,偏航角设置为
通过设置偏航角
来将矢量的坐标/速度分到X轴和Y轴上。
（1）所以k-1时刻的X_true的初始值设置为：
X_true = np.array([[0],[0],[v_0 * math.cos(theta)],[v_0 * math.sin(theta)],[a_0 * math.cos(theta)],[a_0 * math.sin(theta)]])
（2）k-1时刻的状态变量的后验估计可以直接取真实值
X_posteriori = X_true
（3）过程噪声的协方差矩阵Q设置为
（4）测量噪声的协方差矩阵R设置为：
要注意我们只测量X的前两个变量，即X坐标和Y坐标。（定位更关心位置而不是速度）
即：更相信建模值，而较为不信任测量值。
（5）状态转移矩阵A:(t就是时间间隔，可以对其取值)
#状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1,0,t,0,0.5*t*t,0],[0,1,0,t,0,0.5*t*t],[0,0,1,0,t,0],[0,0,0,1,0,t],[0,0,0,0,1,0],[0,0,0,0,0,1]])
#传输矩阵/状态观测矩阵H
H = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0]])
（6）注意传输矩阵H，因为我们只测量了X坐标和Y坐标，所以它的 维度是2*6。
（7）滤波误差的协方差矩阵的初始值对算法影响不大，因为算法迭代的过程，就是协方差矩阵不断变小的过程。初始值我们取：
var_p = 3
P = np.eye(6) * var_p
2.1.4算法迭代
（1）首先是要表达出真实值，注意，真实值=理论值+噪声
X状态空间：
X_true = np.dot(A, X_true) + W
测量值：
Z = np.dot(H, X_true) + V
（2）然后就是五步迭代，可以参考上一篇，不过多赘述。
2.2 Python源代码
import math
import numpy as np
from matplotlib import pyplot as plt
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
#-----------------创建一个函数，用来生成符合正态分布的变量------------------------
def Gaussian_variable_generator(sigma):
y=np.random.normal(loc=0.0,scale=sigma,size=None)
return np.array(y)
#-----------------建立变量-------------------------------------------------
设时间间隔为t
过程噪声的协方差矩阵
t=0.08
G = np.array([1/6*t*t*t,0],[0,1/6*t*t*t],[1/2*t*t,0],[0,1/2*t*t],[t,0],[0,t])
G1 = np.array([],[])
过程噪声的方差
var_q = 0.04
Q = np.eye(6) * var_q
print(Q)
sigma_w1 = math.sqrt(Q[0,0])
sigma_w1 = np.array(sigma_w1)
sigma_w2 = math.sqrt(Q[1,1])
sigma_w2 = np.array(sigma_w2)
sigma_w3 = math.sqrt(Q[2,2])
sigma_w3 = np.array(sigma_w3)
sigma_w4 = math.sqrt(Q[3,3])
sigma_w4 = np.array(sigma_w4)
sigma_w5 = math.sqrt(Q[4,4])
sigma_w5 = np.array(sigma_w5)
sigma_w6 = math.sqrt(Q[5,5])
sigma_w6 = np.array(sigma_w6)
测量噪声的协方差矩阵
因为只有两个测量量：X坐标和Y坐标
R = np.array([[4,0],[0,4]])
sigma_v1 = math.sqrt(R[0,0])
sigma_v1 = np.array(sigma_v1)
sigma_v2 = math.sqrt(R[1,1])
sigma_v2 = np.array(sigma_v2)
#状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1,0,t,0,0.5*t*t,0],[0,1,0,t,0,0.5*t*t],[0,0,1,0,t,0],[0,0,0,1,0,t],[0,0,0,0,1,0],[0,0,0,0,0,1]])
#传输矩阵/状态观测矩阵H
H = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0]])
#----------------------初始化------------------------------------------
#设初始的真实值 X坐标=0 Y坐标=0 初速度v=10 加速度a=2 偏航角theta=pi/3
v_0 = 5
a_0 = 4
theta = math.pi / 3
#利用偏航角 把X_true的各个分量表达出来
X_true = np.array([[0],[0],[v_0 * math.cos(theta)],[v_0 * math.sin(theta)],[a_0 * math.cos(theta)],[a_0 * math.sin(theta)]])
print(X_true)
position_x_true = v_0 * math.cos(theta) * t + 0.5 * a_0 * math.cos(theta) * t * t
position_y_true = v_0 * math.sin(theta) * t + 0.5 * a_0 * math.sin(theta) * t * t
speed_x_true = v_0 * math.cos(theta) + a_0 * math.cos(theta) * t
speed_y_true = v_0 * math.sin(theta) + a_0 * math.sin(theta) * t
acceleration_x_true = a_0 * math.cos(theta)
acceleration_y_true = a_0 * math.sin(theta)
#k-1时刻的状态变量的后验估计可以直接取真实值
X_posteriori = X_true
#假设 k-1 时刻的误差的协方差矩阵为:(误差的协方差矩阵的初始值不是特别重要 反正都会收敛)
var_p = 3
P = np.eye(6) * var_p
#X Y坐标的先验估计值
position_x_prior = []
position_y_prior = []
#X Y方向的速度的先验估计值
speed_x_prior = []
speed_y_prior = []
#X Y方向的加速度的先验估计值
acceleration_x_prior = []
acceleration_y_prior = []
#测量的量只有两个：X坐标和Y坐标
#X坐标的测量量
position_x_measure = []
#Y坐标的测量量
position_y_measure = []
#X Y坐标的后验估计值
position_x_posteriori = []
position_y_posteriori = []
#X Y方向的速度的后验估计值
speed_x_posteriori = []
speed_y_posteriori = []
#X Y方向的加速度的后验估计值
acceleration_x_posteriori = []
acceleration_y_posteriori = []
#X Y坐标的真实值
position_x_true = X_true[0,0]
position_y_true = X_true[1,0]
#X Y方向的速度的后验估计值
speed_x_true = X_true[2,0]
speed_y_true = X_true[3,0]
#X Y方向的加速度的后验估计值
acceleration_x_true = X_true[4,0]
acceleration_y_true = X_true[5,0]
#误差的协方差的迹 用来看是否已经达到最小方差的估计准则了
tr_P_posterior = P[0, 0] + P[1, 1] + P[2, 2] + P[3, 3]+P[4, 4] + P[5, 5]
if __name__ == '__main__':
#迭代次数设为30次
for i in range(100):
# 基于状态空间表达式要建立以下方程(这里全是真实值啊！！)
# 这里要注意 要在循环里面生成每一次的噪声 否则噪声永远都是一个值
w1 = Gaussian_variable_generator(sigma_w1)
w2 = Gaussian_variable_generator(sigma_w2)
w3 = Gaussian_variable_generator(sigma_w3)
w4 = Gaussian_variable_generator(sigma_w4)
w5 = Gaussian_variable_generator(sigma_w5)
w6 = Gaussian_variable_generator(sigma_w6)
W = np.array([[w1], [w2], [w3], [w4], [w5], [w6]])
print(W)
#真实值的表达式
X_true = np.dot(A, X_true) + W
position_x_true = np.append(position_x_true, X_true[0,0])
position_y_true = np.append(position_y_true, X_true[1,0])
speed_x_true = np.append(speed_x_true, X_true[2,0])
speed_y_true = np.append(speed_y_true, X_true[3,0])
acceleration_x_true = np.append(acceleration_x_true, X_true[4,0])
acceleration_y_true = np.append(acceleration_y_true, X_true[5,0])
# 测量噪声
v1 = Gaussian_variable_generator(sigma_v1)
v2 = Gaussian_variable_generator(sigma_v2)
V = np.array([[v1], [v2]])
# 测量值的表达式
Z = np.dot(H, X_true) + V
position_x_measure = np.append(position_x_measure, Z[0,0])
position_y_measure = np.append(position_y_measure, Z[1,0])
----------------------开始循环迭代-------------------------
----------------------时间更新-------------------------
# step1:基于k-1时刻的后验估计值X_posterior建模预测k时刻的系统状态先验估计值X_prior
# 对状态变量进行先验估计
X_prior = np.dot(A, X_posteriori)
# 为了看到Z状态空间各个分量的先验值的变化
position_x_prior.append(X_prior[0,0])
position_y_prior.append(X_prior[1,0])
speed_x_prior.append(X_prior[2,0])
speed_y_prior.append(X_prior[3,0])
acceleration_x_prior.append(X_prior[4,0])
acceleration_y_prior.append(X_prior[5,0])
# step2:基于k-1时刻的误差ek-1的协方差矩阵P_posterior和过程噪声w的协方差矩阵Q 预测k时刻的误差的协方差矩阵的先验估计值 P_prior
P_prior = np.dot(A,np.dot(P, A.T)) + Q
----------------------状态更新-------------------------
# step3:计算卡尔曼增益
K1 = np.dot(P_prior, H.T)
K2 = np.dot(H, np.dot(P_prior,H.T)) + R
K = np.dot(K1, np.linalg.inv(K2))
# step4:利用卡尔曼增益K 进行校正更新状态，得到k时刻的后验状态估计值 X_posterior
X_posteriori = X_prior + np.dot(K, Z - np.dot(H, X_prior))
# 把k时刻后验预测值赋给状态分量的后验预测值
# X Y坐标的后验估计值
position_x_posteriori.append(X_posteriori[0,0])
position_y_posteriori.append(X_posteriori[1,0])
# X Y方向的速度的后验估计值
speed_x_posteriori.append(X_posteriori[2,0])
speed_y_posteriori.append(X_posteriori[3,0])
# X Y方向的加速度的后验估计值
acceleration_x_posteriori.append(X_posteriori[4,0])
acceleration_y_posteriori.append(X_posteriori[5,0])
# step5:更新误差的协方差矩阵
P = np.dot(np.eye(6)-np.dot(K, H), P_prior)
# 误差的协方差矩阵的迹，迹越小说明估计越准确
tr_P_posterior = np.append(tr_P_posterior, P[0, 0] + P[1, 1] + P[2, 2] + P[3, 3] + P[4, 4] + P[5, 5])
# ---------------------再从step5回到step1 其实全程只要搞清先验后验 k的自增是隐藏在循环的过程中的 然后用分量speed和position的append去记录每一次循环的结果-----------------------------
# 可视化显示 画出误差的迹 观察其是否收敛
if True:
# 画出1行2列的多子图
fig, axs = plt.subplots(1,4,figsize=(20,14))
#速度
axs[2].plot(speed_y_true,"-",color="blue",label="Y速度真实值",linewidth="1")
# axs[2].plot(speed_measure,"-",color="grey",label="速度测量值",linewidth="1")
axs[2].plot(speed_y_prior,"-",color="green",label="Y速度先验估计值",linewidth="1")
axs[2].plot(speed_y_posteriori,"-",color="pink",label="Y速度后验估计值",linewidth="1")
axs[2].set_title("Y轴方向速度")
axs[2].set_xlabel('k')
axs[2].legend(loc = 'upper left')
axs[3].plot(speed_x_true, "-", color="blue", label="X速度真实值", linewidth="1")
# axs[2].plot(speed_measure,"-",color="grey",label="速度测量值",linewidth="1")
axs[3].plot(speed_x_prior, "-", color="green", label="X速度先验估计值", linewidth="1")
axs[3].plot(speed_x_posteriori, "-", color="pink", label="X速度后验估计值", linewidth="1")
axs[3].set_title("X轴方向速度")
axs[3].set_xlabel('k')
axs[3].legend(loc='upper left')
font2 = {
'weight': 'bold',
'size': 20,
}
# # 位置
x = position_x_true
y = position_y_true
axs[1].plot(x, y,"-",color="blue",label="位置真实值",linewidth="1")
x1 = position_x_measure
y1 = position_y_measure
axs[1].plot(x1,y1,"-",color="grey",label="位置测量值",linewidth="1")
x2 = position_x_prior
y2 = position_y_prior
axs[1].plot(x2,y2,"-",color="green",label="位置先验估计值",linewidth="1")
x3 = position_x_posteriori
y3 = position_y_posteriori
axs[1].plot(x3,y3,"-",color="red",label="位置后验估计值",linewidth="1")
axs[1].set_title("轨迹图",font2)
axs[1].set_ylabel("Y坐标/m",font2)
axs[1].set_xlabel('X坐标/m',font2)
axs[1].legend(loc = 'upper left')
# 误差的迹
axs[0].plot(tr_P_posterior, "-", color="blue", label="误差的迹", linewidth="3")
axs[0].legend(loc='upper left')
axs[0].set_title("误差的迹", font2)
plt.xticks(fontsize=18, fontfamily='serif')
plt.yticks(fontsize=18, fontfamily='serif')
plt.show()
2.3 结果图分析
在当前的参数设置的情况下，可以看到真实轨迹图（蓝色）大体上符合偏航角为60°的匀加速直线运动的。灰色线条作为测量数据，偏离程度较大；绿色线条作为建模值，偏离程度较小；最后的滤波结果则是更为接近真实值，而且误差的迹也是收敛至最小值了。
然后又研究了一下速度的真实值、先验估计值、后验估计值。
当偏航角是60°时：
当偏航角是30°时：
这说明不同偏航角情况下，相当于对不同方向的加权不一样，导致受到误差的波动影响不一样。
Original: https://blog.csdn.net/ouok000/article/details/125999213
Author: scoutee
Title: 滤波笔记二：运动模型（CV&CA&CTRV）
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/671290/
转载文章受原作者版权保护。转载请注明原作者出处！