2.2卡尔曼滤波

卡尔曼滤波器的预测和更新是卡尔曼滤波算法的两个核心步骤。下面是预测和更新的具体算法步骤：

1. 预测步骤（Predict）：
   - 根据系统模型（状态转移矩阵F）和当前状态估计（状态向量x）进行状态预测。预测得到的状态向量是下一个时间步的状态估计。
   - 根据过程噪声协方差矩阵（Q）进行协方差预测。预测得到的协方差矩阵是下一个时间步的协方差估计。

2. 更新步骤（Update）：
   - 根据测量模型（测量矩阵H）和观测值（测量向量z）进行测量预测。预测得到的测量值是当前时间步的测量值的估计。
   - 根据测量噪声协方差矩阵（R）进行测量协方差预测。预测得到的协方差矩阵是当前时间步的测量协方差的估计。
   - 计算卡尔曼增益（Kalman Gain）。卡尔曼增益是用于融合状态预测和测量预测的权重。
   - 根据卡尔曼增益、观测值与测量预测之间的差异，进行状态更新。更新得到的状态向量是当前时间步的状态估计。
   - 利用卡尔曼增益对协方差进行更新。更新得到的协方差矩阵是当前时间步的协方差估计。

在KalmanFilter类中，预测和更新步骤的实现如下：

1. 预测步骤的实现：
```python
kf.predict()
```

2. 更新步骤的实现：
```python
kf.update(z)
```

其中，kf是KalmanFilter类的对象，z是观测值。调用predict方法进行预测，调用update方法进行更新。

需要注意的是，在预测和更新之前，需要根据具体问题设置卡尔曼滤波器的相关参数，包括状态转移矩阵F、测量矩阵H、过程噪声协方差矩阵Q和测量噪声协方差矩阵R。这些参数的设置需要根据实际问题和系统模型进行调整。

LAMB涡

import numpy as np
import matplotlib.pyplot as plt

def generate_vortex(center, strength, radius, x, y):
    """
    生成单个Lamb涡的速度场
    :param center: 涡旋的中心位置
    :param strength: 涡旋的强度
    :param radius: 涡旋的半径
    :param x: 生成速度场的x坐标网格
    :param y: 生成速度场的y坐标网格
    :return: 速度场的x和y分量
    """
    x_dist = x - center[0]
    y_dist = y - center[1]
    r = np.sqrt(x_dist ** 2 + y_dist ** 2)
    theta = np.arctan2(y_dist, x_dist)

    u = -strength / (2 * np.pi) * radius * np.sin(theta) * np.exp(-(r / radius) ** 2)
    v = strength / (2 * np.pi) * radius * np.cos(theta) * np.exp(-(r / radius) ** 2)

    return u, v

def generate_flow_field(vortices, x, y):
    """
    根据多个Lamb涡叠加生成流场
    :param vortices: 涡旋列表，每个元素为(center, strength, radius)
    :param x: 生成速度场的x坐标网格
    :param y: 生成速度场的y坐标网格
    :return: 速度场的x和y分量
    """
    u = np.zeros_like(x)
    v = np.zeros_like(y)

    for vortex in vortices:
        center, strength, radius = vortex
        u_vortex, v_vortex = generate_vortex(center, strength, radius, x, y)
        u += u_vortex
        v += v_vortex

    return u, v

# 示例使用
# 定义生成流场的网格
x = np.linspace(-10, 10, 100)
y = np.linspace(-10, 10, 100)
X, Y = np.meshgrid(x, y)

# 定义涡旋列表，每个元素为(center, strength, radius)
vortices = [
    ((-5, -5), 1, 2),
    ((5, 5), -1, 3),
    ((0, 0), 0.5, 1),
]

# 生成流场
u, v = generate_flow_field(vortices, X, Y)

# 可视化流场
plt.figure()
plt.quiver(X, Y, u, v)
plt.xlabel('x')
plt.ylabel('y')
plt.title('Flow Field')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def generate_vortex(center, strength, radius, x, y):
    """
    生成单个Lamb涡的速度场
    :param center: 涡旋的中心位置
    :param strength: 涡旋的强度
    :param radius: 涡旋的半径
    :param x: 生成速度场的x坐标网格
    :param y: 生成速度场的y坐标网格
    :return: 速度场的x和y分量
    """
    x_dist = x - center[0]
    y_dist = y - center[1]
    r = np.sqrt(x_dist ** 2 + y_dist ** 2)
    theta = np.arctan2(y_dist, x_dist)

    u = -strength / (2 * np.pi) * radius * np.sin(theta) * np.exp(-(r / radius) ** 2)
    v = strength / (2 * np.pi) * radius * np.cos(theta) * np.exp(-(r / radius) ** 2)

    return u, v

def generate_flow_field(vortices, x, y):
    """
    根据多个Lamb涡叠加生成流场
    :param vortices: 涡旋列表，每个元素为(center, strength, radius)
    :param x: 生成速度场的x坐标网格
    :param y: 生成速度场的y坐标网格
    :return: 速度场的x和y分量
    """
    u = np.zeros_like(x)
    v = np.zeros_like(y)

    for vortex in vortices:
        center, strength, radius = vortex
        u_vortex, v_vortex = generate_vortex(center, strength, radius, x, y)
        u += u_vortex
        v += v_vortex

    return u, v

# 定义深度层及其对应的涡旋特征参数
depths = [0, -100, -200]  # 深度层
centers = [(0, 0), (1, 1), (-1, -1)]  # 涡旋中心位置
strengths = [1, 0.5, 0.8]  # 涡旋强度
radii = [2, 1, 1.5]  # 涡旋半径

# 定义生成流场的网格
x = np.linspace(-10, 10, 100)
y = np.linspace(-10, 10, 100)
z = np.linspace(-300, 0, 100)
X, Y, Z = np.meshgrid(x, y, z)

# 生成三维涡旋流场
u = np.zeros_like(X)
v = np.zeros_like(Y)
w = np.zeros_like(Z)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

for depth, center, strength, radius in zip(depths, centers, strengths, radii):
    u_layer, v_layer = generate_vortex(center, strength, radius, x, y)
    w_layer = np.full_like(u_layer, depth)
    ax.quiver(X, Y, Z+w_layer, u_layer, v_layer, w_layer, length=0.1, normalize=True, color=np.random.rand(3,))
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('depth')
    plt.title('Flow Field')

plt.show()k
卡尔曼滤波

P是协方差矩阵

就是在原来式子的基础上通过化简的形式，转变为矩阵的形式，就得到各个对应的矩阵

V是噪声，考虑测量噪声已包含于角度测量值